NAVAL  POSTGRADUATE  SCHOOL 
Monterey,  California 


20000501  117 


THESIS 

TESTING  AND  EVALUATION  OF  THE  SMALL 
AUTONOMOUS  UNDERWATER  VEHICLE  NAVIGATION 

SYSTEM  (SANS) 

by 

Suat  Arslan 
March  2000 

Thesis  Advisor:  Xiaoping  Yun 

Co-Advisor:  Eric  Bachmann 


Approved  for  public  release;  distribution  is  unlimited 


DTIC  QUALITY  INSPECTED  3 


REPORT  DOCUMENTATION  PAGE 


Form  Approved 
OMB  No.  0704-0188 

Public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instruction, 
searching  existing  data  sources,  gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send 
comments  regarding  this  burden  estimate  or  any  other  aspect  of  this  collection  of  information,  including  suggestions  for  reducing  this  burden,  to 
Washington  headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington,  VA 
22202-4302,  and  to  the  Office  of  Management  and  Budget,  Paperwork  Reduction  Project  (0704-0188)  Washington  DC  20503. 


1.  AGENCY  USE  ONLY  (Leave  blank) 

2.  REPORT  DATE 

3.  REPORT  TYPE  AND  DATES  COVERED 

March  2000 

Master’s  Thesis 

4.  TITLE  AND  SUBTITLE 

TESTING  AND  EVALUATION  OF  THE  SMALL  AUTONOMOUS  UNDERWATER 
VEHICLE  NAVIGATION  SYSTEM  (SANS). 

5.  FUNDING  NUMBERS 

6.  AUTHOR(S)  Arslan,  Suat 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES) 

Naval  Postgraduate  School 

Monterey,  CA  93943-5000 

8.  PERFORMING  ORGANIZATION 
REPORT  NUMBER 

9.  SPONSORING  /  MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 

10.  SPONSORING/  MONITORING 
AGENCY  REPORT  NUMBER 

11.  SUPPLEMENTARY  NOTES 

The  views  expressed  in  this  thesis  are  those  of  the  author  and  do  not  reflect  the  official  policy  or  position  of  the  Department  of 
Defense  or  the  U.S.  Government. 

12a.  DISTRIBUTION  /  AVAILABILITY  STATEMENT  I  12b.  DISTRIBUTION  CODE 


Approved  for  public  release;  distribution  is  unlimited. 


13.  ABSTRACT  (maximum  200  words) 

At  the  Naval  Postgraduate  School  (NPS),  a  small  AUV  navigation  system  (SANS)  was  developed  for  research  in  support  of 
shallow-water  mine  countermeasures  and  coastal  environmental  monitoring.  The  objective  of  this  thesis  is  to  test  and  evaluate 
the  SANS  performance  after  tuning  the  filter  gains  through  a  series  of  testing  procedures. 

The  new  version  of  SANS  (SANS  III)  used  new  hardware  components  which  were  smaller,  cheaper,  and  more  reliable.  A 
PC/104  computer  provided  more  computing  power  and,  increased  the  reliability  and  compatibility  of  the  system. 

Implementing  an  asynchronous  Kalman  filter  in  the  position  and  velocity  estimation  part  of  the  navigation  subsystem 
improved  the  navigation  accuracy  significantly.  To  determine  and  evaluate  the  overall  system  performance,  ground  vehicle 
testing  was  conducted.  Test  results  showed  that  the  SANS  III  was  able  to  navigate  within  ±15  feet  of  Global  Positioning  track 
with  no  Global  Positioning  update  for  three  minutes. 


14.  SUBJECT  TERMS 

INS,  GPS,  AUV, SANS,  Navigation,  Kalman  Filter 


13.  NUMBER 
OF  PAGES 


109 


16.  PRICE  CODE 


17.  SECURITY  CLASSIFICATION 
OF  REPORT 

Unclassified 


18.  SECURITY  CLASSIFICATION  OF 
THIS  PAGE 

Unclassified 


19.  SECURITY  CLASSIFI¬ 
CATION  OF  ABSTRACT 

Unclassified 


20.  LIMITATION 
OF  ABSTRACT 

UL 


NSN  7540-01-280-5500 


Standard  Form  298  (Rev.  2-89) 
Prescribed  by  ANSI  Std.  239-18  298-102 


1 


11 


Approved  for  public  release;  distribution  is  unlimited 


TESTING  AND  EVALUATION  OF  THE  SMALL  AUTONOMOUS 
UNDERWATER  VEHICLE  NAVIGATION  SYSTEM  (SANS) 

Suat  Arslan 

Lieutenant  Junior  Grade,  Turkish  Navy 
B.S.,  Turkish  Naval  Academy,  1993 

Submitted  in  partial  fulfillment  of  the  _ 
requirements  for  the  degree  of 


MASTER  OF  SCIENCE  IN  ELECTRICAL  ENGINEERING 


from  the 


NAVAL  POSTGRADUATE  SCHOOL 
March  2000 


Author: 


Approved  by: 


Xiaoping  Yun,  Thesis  Advisor 

Eric  R.  Bachmann,  Co-Advisor 


Jeffrey  B.  Rnorr,  Chairman 
Department  of  Electrical  and  Computer  Engineering 


iii 


ABSTRACT 


At  the  Naval  Postgraduate  School  (NPS),  a  small  AUV  navigation  system 
(SANS)  was  developed  for  research  in  support  of  shallow-water  mine  countermeasures 
and  coastal  environmental  monitoring.  The  objective  of  this  thesis  is  to  test  and  evaluate 
the  SANS  performance  after  tuning  the  filter  gains  through  a  series  of  testing  procedures. 

The  new  version  of  SANS  (SANS  III)  used  new  hardware  components  which 
were  smaller,  cheaper,  and  more  reliable.  A  PC/104  computer  provided  more  computing 
power  and,  increased  the  reliability  and  compatibility  of  the  system. 

Implementing  an  asynchronous  Kalman  filter  in  the  position  and  velocity 
estimation  part  of  the  navigation  subsystem  improved  the  navigation  accuracy 
significantly.  To  determine  and  evaluate  the  overall  system  performance,  ground  vehicle 
testing  was  conducted.  Test  results  showed  that  the  SANS  III  was  able  to  navigate  within 
±15  feet  of  Global  Positioning  track  with  no  Global  Positioning  update  for  three 
minutes. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Autonomous  Underwater  Vehicles  (AUVs)  are  capable  of  a  variety  of  overt  and 
clandestine  missions.  Such  vehicles  have  been  used  for  inspection,  mine 
countermeasures,  survey,  and  observation  [Ref.  1].  One  of  the  most  important  and 
difficult  aspects  of  an  AUV  mission  is  navigation.  Due  to  intermittent  submergence,  the 
vehicle  does  not  have  continuous  access  to  external  navigational  aids  such  as  the  Global 
Positioning  System  (GPS)  or  Loran.  Thus  the  vehicle  itself  must  be  capable  of 
estimating  the  current  position  from  onboard  sensors  that  measure  speed,  heading, 
velocity  and  acceleration.  Previous  studies  have  shown  that  integrating  GPS  and  INS 
into  a  single  system  makes  possible  production  of  continuously  accurate  navigational 
information  even  when  using  relatively  low-cost  components  [Ref.  11].  GPS  is  capable 
of  supplying  accurate  positioning  when  the  AUV  is  surfaced.  It  is  integrated  with  an  INS 
to  compensate  for  the  loss  of  GPS  signals  when  the  AUV  is  submerged. 

At  the  Naval  Postgraduate  School  (NPS),  a  Small  AUV  Navigation  System 
(SANS)  was  developed  for  research  in  support  of  shallow- water  mine  countermeasures 
and  coastal  environmental  monitoring.  The  goal  was  to  demonstrate  the  feasibility  of 
using  a  small,  low-cost  Inertial  Measurement  Unit  (IMU)  to  navigate  between 
Differential  Global  Positioning  System  (DGPS)  fixes. 

The  first  prototype  of  the  SANS  system  (called  SANS  I)  was  separated  into  two 

subsystems  [Ref.  12].  The  IMU,  water  speed  sensor,  compass,  GPS  antenna,  and  data 

logging  computer  were  housed  in  one  package  and  placed  in  a  towfish.  The  GPS 
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receiver,  DGPS  antenna,  and  data  processing  computer  were  in  the  towing  vessel.  The 
data  collected  by  the  towfish  subsystem  were  transmitted  to  the  processing  computer 
through  a  modem  cable. 

The  second  generation  of  the  SANS  system,  or  SANS  II,  was  totally  contained  in 
a  single  package.  The  software  of  SANS  I  and  SANS  II  was  based  on  a  twelve-state 
constant  gain  complementary  filter.  The  values  of  these  gains  were  initially  selected 
based  on  bandwidth  considerations,  and  later  tuned  based  on  results  from  bench  testing 
and  ground  vehicle  testing. 

SANS  III,  the  current  version  of  SANS,  is  composed  of  the  state-of-the-art 
components,  which  include  an  IMU,  GPS/DGPS  Receiver,  magnetic  compass,  water 
speed  sensor,  and  data  processing  computer.  The  486  based  ESP  computer  used  in  the 
previous  versions  of  SANS  was  replaced  by  an  AMD  586DX133  based  PC/104  computer 
to  provide  more  computing  power,  to  increase  reliability  and  provide  compatibility  with 
PC/104  industrial  standards  [Ref.  4]. 

In  the  current  version  of  SANS,  the  position  and  velocity  estimation  part  of  the 
constant-gain  filter  used  in  SANS  I  and  SANS  II  is  replaced  by  an  asynchronous  Kalman 
filter.  Kalman  filtering  is  a  method  of  combining  all  available  sensor  data,  regardless  of 
their  precision,  to  estimate  the  current  position  of  a  vehicle.  Use  of  a  Kalman  filter 
improves  performance,  decreases  navigational  errors,  and  improves  reliability  relative  to 
filters  that  depend  only  on  one  sensor  input. 

B.  RESEARCH  OBJECTIVES 

The  objectives  of  this  thesis  research  are  the  following: 

-  to  implement  Kalman  filtering  within  the  SANS  III, 
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-  to  improve  navigation  accuracy  of  the  SANS  III  by  properly  tuning  filter  gains, 
-to  test  and  evaluate  the  overall  performance  of  the  SANS  III  through  ground 
vehicle  testing  in  preparation  for  the  at-sea  testing. 

C.  SCOPE,  LIMITATIONS,  AND  ASSUMPTIONS 

This  thesis  reports  part  of  the  findings  of  more  than  eight  years  of  research  in  an 
ongoing  project.  The  main  objective  of  this  thesis  is  to  test  and.  evaluate  the  current 
configuration  of  the  SANS  through  bench  and  ground  vehicle  tests,  and  tune  the  Kalman 
filter  gains  to  increase  reliability  and  accuracy  of  the  system  before  at-sea  testing. 

D.  ORGANIZATION  OF  THESIS 

Chapter  II  provides  an  overview  and  description  of  the  current  hardware  and 
software  components  of  the  system. 

Chapter  III  describes  the  Kalman  filter  for  the  SANS  III. 

Chapter  IV  describes  system  test  procedures  and  results. 

Chapter  V  presents  the  thesis  conclusions  and  provides  recommendations  for 
future  research. 
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II.  SYSTEM  OVERVIEW 


A.  INTRODUCTION 

References  [13  and  14]  include  detailed  information  about  the  current  SANS 
hardware  and  software  components.  The  purpose  of  this  chapter  is  to  present  an 
overview  of  the  configuration  of  SANS  III  hardware  and  software  units. 

B.  SANS  III  HARDWARE  OVERVIEW 

Most  of  the  previous  SANS  hardware  parts  have  been  replaced  with  more 
powerful,  flexible,  and  reliable  components  which  are  faster,  smaller,  and  cheaper. 
Figure  2.1  shows  the  SANS  III  hardware  configuration.  The  main  components  are  a  Real 
Time  Devices  133  MHz  AMD  586  PC/104  module,  a  Sealevel  C4-104  four-port  PC/104 
compatible  RS-232  module,  a  Real  Time  Devices  DM406/DM5406  analog  to  digital 
(A/D)  converter,  a  Real  Time  Devices  CMT104  IDE  hard  drive  module  and  data  sensors 
which  include  the  Crossbow  DMU-VG  Six  Axis  IMU,  the  Motorola  Oncore/McKinney 
Technology  GPS/DGPS  Receiver  unit,  the  Precision  Navigation  TCM2  Electronic 
Compass,  and  the  B&G  Sonic  Water  Speed  Sensor. 

1.  Real  Time  Devices  AMD  586DX133  Based  PC/104 

The  Real  Time  Devices  AMD  586DX133  based  PC/104  is  being  used  as  a  data 
acquisition  and  processing  unit  in  SANS  III  system.  PC/104  is  an  industrial  standard  for 
creating  embedded  computer  applications.  It  fulfills  the  basic  needs  of  embedded  systems 
such  as  low  power  consumption,  modularity,  small  foot  print,  high  reliability,  good  noise 
immunity,  high-speed  operation,  and  expandability  [Ref.  13]. 
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Figure  2.1  Current  SANS  Hardware  Configuration  [After  Ref.  13] 

The  PC/ 104  can  be  easily  customized  by  stacking  PC/ 104  modules  that  are 
compliant  with  the  PC/104  bus  architecture,  such  as  video  controllers,  network  interfaces, 
analog  and  digital  data  acquisition  modules,  sound  I/O  modules,  etc  [Ref.  4]. 

The  SANS  system  is  equipped  with  five  PC/104  modules.  These  are  the  CPU 
module,  the  hard  drive  module,  the  VGA  module,  A/D  converter  and  the  serial  I/O 
module. 

2.  Sealevel  C4-104  Serial  I/O  Module 

The  PC/104  compatible  Sealevel  C4-104  serial  I/O  module  provides  four  RS-232 
serial  I/O  ports  for  connecting  the  four  sensors  of  SANS  III  system.  Each  serial  port  can 
be  configured  to  have  its  own  base  memory  address  and  Interrupt  Request  (IRQ) 
assignment. 
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The  C4-104  is  compliant  with  PC/104  specification  regarding  both  mechanical 
and  electrical  specifications.  The  C4-104  utilizes  Universal  Asynchronous 
Receiver/Transmitters  (UART)  with  programmable  baud  rates,  data  format,  interrupt 
control  and  a  16-byte  input  and  output  FIFO  [Ref.  7]. 

3.  Real  Time  Devices  DM406/DM5406  Analog  to  Digital  (A/D)  Converter 

The  DM406/DM5406  A/D  Converter  can  receive  up  to  8  differential  or  16  single 

-ended  analog  inputs.  It  converts  these  inputs  into  12-bit  digital  data  words.  The  analog 
input  voltage  range  is  jumper-selectable  for  bipolar  ranges  of -5  to  +5  volts,  -10  to  +10 
volts,  or  a  unipolar  range  of  0  to  +10  volts.  The  module  is  provided  with  overvoltage 
protection  to  +  35  volts. 

A/D  conversions  are  performed  in  5  microseconds,  and  the  maximum  throughput 
rate  is  1 00  kHz.  Conversions  can  be  controlled  through  software,  by  an  on-board  pacer 
clock,  or  by  an  external  trigger  brought  onto  the  board  through  the  I/O  connector. 

The  converted  data  can  be  transferred  to  PC  memory  through  the  PC  data  bus  or 
by  using  direct  memory  access  (DMA).  Detailed  information  for  settings  and 
programming  the  module  is  supplied  in  [Ref.  8], 

4.  Crossbow  DMU-VG  Six  Axis  Inertial  Measurement  Unit 

The  DMU-VG  is  a  six-axis  measurement  system  designed  to  measure  linear 
acceleration  along  three  orthogonal  axes,  and  rotation  rates  around  three  orthogonal  axes. 
It  is  designed  to  provide  stabilized  pitch  and  roll  in  dynamic  environments  [Ref.  10].  The 
IMU  has  both  analog  output  and  RS-232  serial  port  output. 

.The  DMU-VG  is  designed  to  operate  in  one  of  two  modes,  a  scaled  sensor  mode 
and  a  voltage  mode.  It  was  configured  to  operate  in  voltage  mode  for  the  SANS. 
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5.  Motorola  Onco re/McKinney  Technology  GPS/DGPS  Receiver 

The  GPS  receiver  unit  used  in  the  SANS  system  is  based  upon  the  Motorola 
ONCORE  Receiver  [Ref.  9].  It  is  capable  of  tracking  eight  satellites  simultaneously. 
The  GPS  receiver  incorporates  a  DGPS  capability.  The  DGPS  receiver  unit  was  specially 
designed  by  McKinney  Technology  to  operate  in  the  Monterey  Bay  area.  Its  data  port 
interface  is  RS-232  compatible.  The  output  message  consists  of  latitude,  longitude, 
height,  velocity,  heading,  time,  and  satellite  tracking  status.  The  test  results  conducted 
show  that  the  GPS  unit  with  differential  corrections  can  provide  a  positional  accuracy  of 
15  ft. 

6.  Precision  Navigation  TCM2  Electronic  Compass 

The  Precision  Navigation  model  TCM2  Electronic  Compass  Module  is  used  in 
the  current  SANS  hardware  configuration  [Ref.  3].  The  TCM2  consists  of  a  three-axis 
magnetometer,  a  two-axis  tilt  sensor  and  a  small  A/D  board.  The  tilt  angle  compensation 
of  the  sensor  depends  on  the  TCM2  model  (20,  50,  and  80  degrees).  Output  may  include 
heading,  roll,  pitch,  temperature,  and  a  three  dimensional  magnetic  field  measurement. 
The  TCM2  standard  output  format  can  be  configured  to  provide  those  parameters 
required  by  the  user.  Precision  Navigation  literature  lists  accuracy  as  within  one  half  of  a 
degree  in  level  operation.  The  TCM2  can  be  calibrated  for  local  magnetic  field 
distortions  by  the  user.  It  provides  an  alarm  in  the  output  data  when  local  magnetic 
anomalies,  are  present,  and  gives  out-of-range  warnings  when  the  unit  is  being  tilted  too 
far.  The  calibration  of  the  compass  and  its  error  characteristics  are  described  in  [Ref  3]. 
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7.  B&G  Microsonic  Speed  Sensor 

The  B&G  Water  Speed  Sensor  (Figure  2.2)  has  an  accuracy  of  0.05  Knots.  It  can 
measure  the  speeds  up  to  30-40  Knots  depending  on  the  conditions  of  the  signal  path. 


Figure  2.2  Water  Speed  Sensor 

The  sensor  transmits  groups  of  ultrasonic  pulses  (short  hurts  of  500  kHz)  from 
one  transducer  to  the  other,  swapping  transducers  after  each  group.  Each  received  pulse 
precisely  triggers  the  next  transmitted  pulse.  The  total  time  taken  for  each  group  depends 
on  the  propagation  time  in  the  sonic  path,  hence  variations  in  group  time  caused  by  water 
flow  between  the  transducers  can  be  measured  and  converted  to  the  speed  of  water  flow. 
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The  microprocessor  continually  monitors  the  received  pulses,  adjusting  the  gain 
of  the  receiver  in  response  to  changes  in  propagation  conditions  (e.g.  air  bubbles)  and 
also  detects  faulty  sonic  conditions. 

The  size  of  the  sensor  is  also  appropriate  for  the  concept  of  the  SANS.  It  has  an 
overall  length  of  sixteen  inches,  and  a  height  of  three  inches. 

C.  SANS  III  SOFTWARE  OVERVIEW 

The  software  of  the  SANS  III  was  written  in  C++  and  compiled  using  the  Borland 
5.0.  It  is  designed  to  utilize  IMU,  heading,  and  water-speed  information  to  implement  an 
INS  based  on  an  asynchronous  Kalman  filter.  It  also  integrates  GPS  information  with 
INS  information  to  obtain  continuously  accurate  navigation  information  in  real  time.  The 
main  flow  between  modules  of  the  system  is  shown  in  Figure  2.3. 


Figure  2.3  SANS  Software  Main  Modules 
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The  modules,  with  the  exception  of  the  water  speed  sensor,  take  in  the  data 
through  the  serial  ports  and  supply  it  to  the  main  program,  toefish,  for  screen  set  up  and 
output.  The  Sealevel  C4-104  module  provides  serial  port  connections  for  the  devices. 
Water  speed  data  is  supplied  to  the  system  through  the  A/D  converter. 

The  GPS  class  object  obtains  GPS  position  messages  over  a  RS-232  interface  via 
the  COM1  serial  port.  The  GPS  data  message  length  is  76  bytes  long.  The  message 
should  have  proper  header  and  a  valid  checksum  in  order  to  be  recognized  as  a  valid 
message. 

The  compass  data  is  received  over  a  RS-232  interface  via  the  COM2  serial  port. 
The  message  length  for  compass  data  is  60  bytes  long.  The  TCM2  compass  is  configured 
to  supply  magnetic  heading  information.  Magnetic  declination  [Ref.  18]  is  added  to 
determine  the  true  north.  For  Monterey  Bay,  this  value  is  15  degrees  and  6  minutes  [Ref. 
17].  It  depends  on  location  (latitude,  longitude)  and  year. 

IMU  data  is  received  over  a  RS-232  interface  via  the  COM3  serial  port.  The 
Crossbow  IMU  runs  in  VG  mode.  The  IMU  message  has  22  bytes  of  data.  The  data 
packet  consists  of  stabilized  pitch  and  roll  angles  along  with  angular  rate  and  linear 
acceleration  information  [Ref.  13]. 

The  water  speed  sensor  data  is  received  through  an  A/D  converter.  The  output  of 
the  sensor  is  voltage. 

The  INS  class  implements  the  inertial  navigation  portion  of  the  SANS.  It  is  based 
on  an  asynchronous  Kalman  filter.  The  INS  class  instantiates  a  Sampler  object  from 
which  it  obtains  heading,  speed,  linear  acceleration  data,  and  angular  rate  data.  GPS 
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information  is  also  passed  to  the  INS  class  via  the  Navigator  object.  The  INS  produces 
accurate  navigation  information  by  integrating  IMU  data  and  DGPS  data  [Ref  13]. 

The  Sampler  prepares  raw  IMU,  heading  and  water  speed  data  for  use  by  the  INS 
object.  The  Sampler  controls  the  data  formatting  and  returns  a  formatted  sample  if  valid 
raw  data  is  available.  Otherwise,  a  negative  response  is  returned. 

The  navigator  object  interfaces  with  both  the  GPS  and  INS  .objects  to  determine  if 
they  have  an  updated  estimate  of  the  current  position,  and  provides  an  estimate  of  the 
current  position  in  hours,  minutes,  seconds  and  milliseconds  of  latitude  and  longitude. 
If  GPS  information  is  available,  the  navigator  object  converts  a  latitude  and  longitude 
expressed  in  milliseconds  to  a  grid  position  in  feet  and  passes  it  to  INS  class,  so  that  the 
INS  object  can  calculate  the  current  position  estimate  with  new  GPS  information.  If  no 
GPS  information  is  available,  the  INS  object  calculates  the  current  position  estimate  by 
using  Kalman  Filter  equations. 

The  software  description  for  configuring  the  Sealevel  C4-104  four  RS-232  serial 
ports  can  be  found  in  [Ref.  13]. 

D.  SUMMARY 

The  components  in  the  current  SANS  hardware  configuration  were  chosen  based 
on  size,  cost,  power,  and  ease  of  operation.  The  new  components  reduced  the  size  of  the 
system  by  52%  [Ref.  14].  The  C4-104  Serial  I/O  Module  provided  four  RS-232  serial 
ports  for  the  PC/1 04  application.  The  various  test  results  of  the  SANS  III  showed  that  the 
AMD  5S6DX133  based  PC/104  computer  provided  more  computing  power  and,  more 
importantly,  increased  reliability  and  compatibility  with  PC/104  industrial  standards. 
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The  technological  advances  in  sensors  make  it  possible  to  change  out  the  current 
components  of  the  system  for  the  newer  versions  in  future  years.  With  this  feature,  more 
accurate  navigation  information  can  be  acquired,  as  well  as  smaller  size  advantages. 

All  additions  and  updates  to  the  SANS  software  were  compiled  under  the  Borland 
version  5.0,  C44  compiler.  The  software  runs  on  a  DOS  (standard)  platform  with  an 
AMD  5 86DX/1 33  MHz  processor. 

Since  DGPS  information  is  available  aperiodically  due  to  asynchronous 
reacquisition  time  of  satellite  signals  and  asynchronous  submergence  and  surfacing 
duration  of  the  AUV,  an  asynchronous  Kalman  filter  is  needed  to  optimally  integrate 
IMU  and  DGPS  data.  A  Kalman  filter  was  implemented  in  the  INS  object  to  improve  the 
accuracy  of  SANS. 

Minor  changes  have  been  made  in  the  SANS  software.  The  changes  were  mostly 
related  to  the  Kalman  filter  part  of  the  system.  The  gains  and  constants  were  tuned  to 
obtain  improve  accuracy. 

Most  of  the  SANS  software  is  still  same  as  in  Appendices  A  and  B  of  [Ref.  13]. 
The  modified  part  of  the  SANS  software  is  presented  in  Appendix  A. 
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III.  SANS  ASYNCHRONOUS  KALMAN  FILTER 


A.  INTRODUCTION 

The  objective  of  the  SANS  is  to  produce  real  time  navigation  information  by 
integrating  an  Inertial  Navigational  System  with  a  Differential  Global  Positioning  System 
(DGPS).  The  navigation  software  in  the  previous  versions  of  SANS  was  based  on  a 
twelve-state  constant  gain  filter.  Filter  gains  were  initially  selected  based  on  bandwidth 
considerations  and  later  tuned  based  on  tilt  table  and  bench  testing.  This  filter  is  being 
replaced  by  an  asynchronous  Kalman  filter  in  SANS  III.  This  chapter  discusses  the 
developed  discrete  time  asynchronous  Kalman  filter  (Figure  3.1)  that  estimates  the  eight 
states  of  the  SANS  III  model  after  attitude  estimation.  A  brief  discussion  will  be 
provided  on  Kalman  filter  basics  leading  into  a  derivation  of  the  SANS  III  Kalman  filter 
equations.  - 

B.  KALMAN  FILTER  BASICS 

The  Kalman  filter  is  a  recursive  predictive  update  technique  used  to  estimate  the 
states  of  a  process  which  can  be  defined  as  a  discrete-time  or  continuous-time  model.  For 
our  purpose,  we  will  study  the  discrete-time  model  of  the  Kalman  filter.  This  state-space 
filter  method  has  two  main  features  (1)  vector  modeling  of  the  random  processes  under 
consideration,  and  (2)  recursive  processing  of  the  noisy  measurement  (input)  data  [Ref. 
2].  Given  some  initial  estimates,  it  allows  the  states  of  a  model  to  be  predicted  and 
adjusted  with  each  new  measurement.  It  also  provides  an  estimate  of  error  at  each 
update!  Kalman  filters  incorporate  the  effects  of  measurement  noise  and  modeling  noise 
in  their  computational  structure. 
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Figure  3.1  SANS  Navigation  Filter  [From  Ref.  13] 
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We  assume  that  the  random  process  to  be  estimated  can  be  modeled  in  the  form  : 

x*+1=<z&*+wt  (3.1) 

In  cases  where  the  relationship  between  model  and  its  measurements  is  linear,  the 

observation  (measurement)  of  the  random  process  model  (Eq.  3.1)  becomes: 

zi=Hytxi+vi  (3.2) 

Throughout  the  remainder  of  this  chapter,  various  terms  are  usecT  to  better  explain  the 

Kalman  filter.  The  following  notations  are  consistent  with  [Ref.  2]: 

xk  (nxl)  Vector  containing  actual  states  of  a  process  model  at  time  tk . 
xk  (nxl)  Vector  containing  the  current  estimated  states  at  tk . 
x~  (nxl)  Vector  containing  the  estimated  states  at  time  t  before  updating  with 
measurement. 

£~+]  (nxl)  Vector  containing  the  estimated  states  at  the  next  time  sample. 

(n  x  n)  Matrix  containing  the  error  covariance  for  x* . 
p~  (n  x  n)  Matrix  containing  the  error  covariance  for  . 

PA'+1  (n  x  n)  Matrix  containing  the  error  covariance  for  ij+l . 
zk  (m  x- 1)  Vector  containing  the  measurement  at  time  tk  . 

H*  (m  x  n)  Matrix  giving  the  ideal  (noiseless)  connection  between  the  measurement 
and  the  state  vector  at  time  tk  . 

Rk  (m  x  n)  Matrix  denoting  the  measurement  error  covariance,  which  must  be  known 
or  estimated  a  priori. 

<f>k  (n  x  n)  Matrix  containing  the  model  relating  xk  and  i‘+1  in  the  absence  of  a  forcing 
function  ( if  xk  is  a  sample  of  a  continuous  process,  <f>k  is  the  usual  state  transition 
matrix). 

wk  (nxl)  Vector  whose  elements  are  white  sequences  with  known  covariance 
structure  to  be  used  with  x* . 

v*  (m  x  1)  Vector  whose  elements  are  the  measurement  errors  associated  with  zk  - 
assumed  to  be  a  white  sequence  with  known  covariance  structure  and  having  zero 
cross-correlation  with  the  wk  sequence. 

Q  k  '  (h  x  n)  Matrix  containing  the  covariance  of  . 

Kk  (n  x  n)  Kalman  Gain  matrix  that  relates  the  amount  of  influence  that  the  error 
between  i;  and  zk  has  in  deriving  xk  ■ 
t  j  time  constant  of  a  given  state  vector  x* 
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qi  white  noise  variable  with  zero  mean  and  variance  of  element  being  modeled. 

D,  zero  frequency  white  noise  density,  (magnitude  of  q{) 

The  basic  discrete-time  Kalman  filter  contains  five  recursive  equations. 

Beginning  with  a  prior  estimate,  the  noisy  measurement  zk  is  used  with  a  blending  factor 

K^to  improve  the  the  estimate  as  follows; 

xk  =  xk  +  Kk(zk-  H^xj)  ~  (3-3) 

K*  (known  as  the  Kalman  gain)  is  now  needed  to  find  the  optimal  estimate  xk  .  It 

takes  the  error  covariance  (mean-square  error)  between  the  current  state  xk  and  the 

estimated  state  xk  and  applies  it  with  H*  and  Rt  resulting  in 

K*  =P;H[(HjtP;H[  +Ri)1  (3.4) 

Once  the  Kalman  gain  Kk  minimizes  the  mean-square  estimation  error,  a  new 

covariance  matrix  can  be  computed  for  xk  (Eq.  3.3)  with 

p»  =(I-K,H,)P;  (3.5) 

The  updated  estimate  x*  is  projected  ahead  using  the  state  transition  matrix  <f>k . 

Unlike  Equation  3.1  the  noise  vector  wk  does  not  affect  the  projected  states  because  it 

has  zero  mean  and  is  white  (zero  correlation  with  any  previous  xvk ).  Thus, 

K+i  =  </>kxk  (3-6) 

Finally,  the  projected  error  covariance  for  x^+]  uses  the  updated  error  covariance 
P4  front  Equation  3.5,  the  covariance  of  wk  in  Equation  3.1,  denoted  as  Qk,  and  <j>k  the 
state  transition  matrix  to  form  the  following: 

I*A+1  =(f>lFk(t>k  +Q*  (3-7) 
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Equations  3.3  through  3.7  can  be  placed  into  an  algorithm  that  can  loop  indefinitely.  This 
Kalman  filter  loop  is  shown  in  Figure  3.2. 


Enter  prior  estimate  x0  and  its  error  covariance  P0 


Figure  3.2  Kalman  Filter  Loop  [From  Ref.  2] 

C.  DERIVATION  OF  SANS  III  KALMAN  FILTER  EQUATIONS 

The  SANS  III  asynchronous  Kalman  filter  has  six  states  for  orientation  estimation 
(still  constant  gain),  and  eight  states  for  position  estimation.  The  part  of  the  filter  for 
orientation  estimation  remains  the  same  as  reported  in  [Ref.  11].  Figure  3.3  shows  the 
process  model  developed  by  the  SANS  team  [Ref.  11].  In  this  model,  the  velocity 
relative  to  water,  ocean  current,  and  GPS  bias  (state  variables  x,  through  x6)  are 
modeled  as  colored  signals  generated  by  white  noises  ql,...,q6  through  first-order 
systems.  The  velocity  relative  to  the  ground  is  obtained  by  summing  the  velocity  relative 
to  water'  (x,  and  x2)  and  the  ocean  current  (x3  and  x4).  This  result  is  integrated  to 
obtain  position  estimation  (x7  and  xg). 
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Figure  3.3  SANS  Process  Model  (after  attitude  estimation)  [From  Ref.  11] 

Low-frequency  DGPS  noise  is  explicitly  modeled  based  on  an  experimentally 
obtained  autocorrelation  function.  Ocean  currents  are  modeled  as  a  low-frequency 
random  process.  Finally,  the  asynchronous  nature  of  the  DGPS  measurements  resulting 
from  AUV  submergence  or  wave  splash  on  the  DGPS  antenna  is  also  taken  into  account 
by  adopting  an  asynchronous  Kalman  Filter  as  the  basis  for  the  SANS  position  estimation 
software. 

Therefore,  the  Kalman  filter  state  equations  are  characterized  by: 


1  1 

—  Xj  H - 

Tj  Tj 


(3.8) 


1  1 

- x,+—q2 

X2  T2 


(3.9) 
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(3-10) 


1  1 

*3  = - *3+—  ?3 

T,  X, 


1  1 

*4  = - *4+—  04 

X4  t4 

(3.11) 

1  1 

*5  = - x5+—q5 

*5  ^5 

(3.12) 

1  1 

*6  = - x6+—q6 

■ 

(3.13) 

Xj  =  X,  +  x3 

(3.14) 

Xg  =  x2  +  x4 

(3.15) 

x j  represents  the  time  constant  of  the  state  vector  x. .  r,  and  t2  are  the  speed 
through  water  time  constants,  r3  and  r4  are  the  water  current  time  constants,  and 
r5  and  r6  are  the  GPS  time  constants. 

Acceleration  signals  from  the  attitude  estimation  part  of  the  SANS  III  filter  were 
not  used  for  velocity  estimation  because  they  were  judged  to  be  too  noisy  to  provide 
useful  information  in  comparison  to  values  for  velocity  obtained  directly  from  an 
accurate  water  speed  sensor.  With  the  process  model  shown  in  Figure  3.3,  the 
measurements  used  for  position  estimation  are  the  velocity  relative  to  water  provided  by 
the  water  speed  sensor  and  position  information  provided  by  DGPS.  The  velocity 
measurements  are  synchronous  and  available  at  every  sampling  time.  DGPS  information 
is  asynchronous  and  is  only  available  when  the  AUV  is  surfaced.  The  two  synchronous 
measurement  equations  are: 

Zj=x  i+v,  (3.16) 
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z2  =x2  +  v2 


(3.17) 


where  v/5  i=l,2  are  white  noise.  That  is,  it  is  assumed  that  the  velocity  measurements 
contain  additive  white  noise.  The  two  asynchronous  measurement  equations  are: 

z3=x7+x5  (3.18) 

z4  =x8  +x6  (3.19) 

From  these  equations,  fa  (state  transition  matrix)  was  developed. 

xx  = — —x1+—q1  (3.20) 

T,  T, 

By  taking  Laplace  transform  of  Equation  3.20: 

sX1(s)-x1(0)  =  -—x1(s)  +  —q1(s) 

X,  X, 

(s  +  -)Xl(s)  =  x](0)  +  -q,(s) 
x,  x, 

X\  (s)—  — -j  X]  (0)  + - ^^(5)  (3.21) 

(5  +  —)  ^(^  +  — ) 

Ti  h 

By  taking  inverse  Laplace  transform  of  Equation  3.21: 

-—t  2  •'  -—(t-x) 

Xy(t)  =  e  T‘  x{(0)-\ - \e  T‘  qx(x)dx 

Ti  o 

So  the  discrete  time  model  can  be  written  as: 

--to  l  '**>  — —( tM-x) 

xx(tk+l)  =  e  Tl  xl(tk)  +  —  \ez'  q}(x)dx,  At  =  tk+l-tk 

-  •  Ti  z 

Similarly,  the  discrete  time  model  for  Equations  3.8  through  3.13  can  be  developed  as: 
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-7*  1  '*? 

*/'*+!>  =  «'  xi(tk)  +  —\  e"  q,(T )dx  i  =  2, 3, 4,5, 6  (3.22) 

X  * 


X/  4 


Next,  we  need  to  develop  the  discrete  time  model  for  Equations  3.14  and  3.15. 


Taking  the  Laplace  Transform  of  x7  =  x,  +  x3 ; 


sX1(s)-x1(0)  =  Xx(s)  +  Xl(s) 


_  *i(0)  Ui(s)  ,  X3(0)  ,  ttj(s) 

_  _+ - — + — _+ - — 

5  +  —  ^(sH - )  S  +  —  X3(,S  + — ) 


x,(s)=1-x,(0)+-^-+-uJ.(l\  +  4W  +_«Js)  (323) 

S  5(5  +  — )  T1sfs  + — )  +  — )  X2s( S  H — — 


Finding  inverse  Laplace  transform  of  - 


1 


s(s  +  ~) 

T 


1 _ Kt_+  K2  _  K\(s+  t)  +  K2s 


s(s  +  —)  S  5  +  —  sf  S  +  -,) 

X  X  T 


K,  =  -K2 

X 


Kx  =  x  and  K2  =  -x 


1 


x  x 


1 

-/ 


,  i  s  1 

s(s  +  ~)  S +  - 

T  X 


x-xeT  =  xfl-cT  ) 


(3.24) 


By  using  the  Equation  3.24,  we  can  take  the  inverse  Laplace  Transform  of 
Equation  3.23: 
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_JL, 


(<~r) 


x7(t)  =  x2(0)  +  xl(l-e  T*  )xl(0)+  JYl-e  Tl  )qx(x)dx 


-1, 


-O-Tj 


+  x3  (i  -  e  T3  >3  (0,)  +  JYl  -  e  t3  ^3  ( t M  (3.25) 

o 

Likewise,  solution  for  Equation  3.15  can  be  found  as: 


x&(t)  =  xz(0)  +  x2(\-e  12  )x2(0)+  j(l-e  T2  )q2(x)dx 


+  xA(l-e z*')x4(0)  +  )q4(x)dx  (3.26) 


and  the  discrete  time  model  for  Equations  3.14  and  3.15  can  be  modeled  by  using 
Equations  3.25  and  3.26: 


A/ 


/•  (*k+ 1  *) 

x7(tk+i)  =  x1(tk)  +  x1(l-e  X>)x](tk)+  \(l-e  T‘  )ql(x)dx 


/jt+1  -—(tk+ i~t) 

+  x3(l-e  X,)x3(tk)+  j(l-e  *  )q3(x)dx  (3.27) 


A/ 


+  X')x2(tk)+  J(l-eT2  )q2(x)dx 


A/ 


- -  -  j 

+  ^0-6  u)xjtk)+  \(\-e  T4  +  )q,(x)dx  (3.28) 


Therefore,  the  discrete  process  model  of  the  system  is  given  by 

x(tk+x)  =  ^kx(tk)  +  w(tk) 
Where  the  state  transition  matrix  <t>t  has  the  form  of 


(3.29) 
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And  the  discrete  white  noises  are  given  by 


l  f  ~(lk+l~x) 

'i(h)  =  —  \e  T'  <h(^)dx,  i=l,2 ,...,6 


0 

0 

0 

0 

0 

0 

0 

1 


1  _ Lft,  .  — r ) 

wi(h)=  }  1-e  T|  ql(x)dx  +  j 

'*  L  J  h 


~(h+\-i) 

l  —  e  q3(x)dx 


(3.30) 
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It  is  noted  that  qt(t),  i=l, 2,3, 4,5,6  are  continuous  independent  white  noise  sources  with 
zero  mean  and  variance  Di .  Thus; 

E[qi(t)qJ(x)\=  0,i*j 
E[qi(t)qi(x)\=Dib(t-x) 

Next,  the  covariance  matrix  Qk of  w (tk )  is  computed: 

Qk  =  E[w(tk)wT(tk)] 

By  noting  the  expression  of  w,(tt)  from  the  previous  page,  Qk  should  have  the 
following  form : 

qu  0  0  0  0  0  qX7  0 

0  ?22  0  0  0  0  0  #28 

0  0  q33  0  0  0  g37  0 

0  0  0  q44  0  0  0  q4g 

^  0  0  0  0  qS5  0  0  0 

0  0  0  0  0  q66  0  0 

q7]  0  q73  0  0  0  q77  0 

_  0  qS2  0  q84  0  0  0  #gg_ 


- (4+ 1“£)  (/*+i”77) 
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?28  =  4 82  =  E[W2  (h  K  (tk  )]  =  D2 
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and  v*  is  measurement  noise  with  covariance  R 
".5  0  0  O' 

0  .5  0  0  . ,  ____  .  . 

R  =  with  DGPS  signal 

0  0  0  0 
'  _0  0  0  0_ 

R  =  ^  without  DGPS  signal 

_0  .5_ 

The  H  matrix  is  the  noiseless  connection  between  measurement  and  the  state  vector  at 
time  t.  For  the  SANS  asynchronous  Kalman  filter,  two  H  matrices  describe  this 
connection,  one  for  samples  with  DGPS  the  other  for  samples  without  DGPS. 

'1  0  0  0  0  0  0  O' 

0  1  0  0  0  0  0  0  . ,  __  .  , 

H  =  with  DGPS  signal 

0  0  0  0  1  0  1  0 

0  0  0  0  0  1  0  1 

ri  0  0  0  0  0  0  01  .  , 

H  =  without  DGPS  signal 

_0  1  0  0  0  0  0  0_ 
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The  SANS  Kalman  filter  Model  that  was  developed  above,  was  tested  in 
MATLAB.  The  model  was  assumed  stationary  throughout  the  simulation.  Simulation 
results  of  the  SANS  Kalman  filter  are  presented  in  the  following  chapter. 

The  MATLAB  source  code  for  the  SANS  Kalman  filter  is  presented  in  Appendix 
B. 
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IV.  SYSTEM  CALIBRATION  AND  TESTING 


A.  INTRODUCTION 

This  chapter  presents  the  calibration  of  system  components,  and  the  testing  results 
of  the  current  SANS  configuration.  After  integrating  the  new  hardware  and 
implementing  the  new  software,  calibration  of  individual  devices  was  required  to  achieve 
better  results  before  testing  the  overall  system.  Bench  testing  was  performed  to 
determine  the  functionality  and  accuracy  of  the  entire  system.  Ground  vehicle  testing, 
with  the  SANS  system  was  mounted  on  a  moving  golf  cart,  was  conducted  to 
demonstrate  the  feasibility  of  the  SANS  system  and  to  observe  system  performance 
before  at-sea  testing.  These  tests  were  also  conducted  to  further  tune  the  gains  and 
constants  of  the  SANS  Kalman  filter. 

Matlab  simulation  results  and  hardware  bench  testing  results  of  the  SANS 
Kalman  filter  are  also  presented  in  this  chapter. 

B.  CALIBRATION  OF  IMU  AND  COMPASS 

1.  Tilt  Table  Calibration  of  IMU 

In  order  to  calibrate  the  scale  factors  of  accelerometers  and  determine  K1  of  the 
12-state  complementary  filter,  the  IMU  unit  was  placed  on  a  Haas  rotary  tilt  table,  model 
TRT-7  (Figure  4.1).  The  table  has  two  degrees  of  freedom  (DOF)  and  is  capable  of 
positioning  to  an  accuracy  of  0.001  degrees  at  rates  ranging  from  0.001  to  80  degrees/s 
[Ref.  16], 
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Tuning  data  for  the  IMU  was  obtained  by  moving  the  unit  through  each  DOF  at 
varying  rates  within  a  45  degree  range.  The  attitude  as  determined  by  the  unit  was  then 
plotted  and  compared  with  the  actual  motion  of  the  table.  Through  this  comparison,  it 
was  possible  to  determine  initial  gain  values  and  scale  factors.  The  tilt  table  data  was 
then  postprocessed  using  these  initial  values  and  once  again  compared  to  the  actual 
motion  of  the  table.  This  process  was  repeated  several  times  until  the  attitude  determined 
by  the  filter  “matched”  the  true  motion  of  the  table. 

Figure  4.2  shows  an  example  of  the  results  obtained  during  the  timing  process. 
The  trajectory  of  the  tilt  table  is  not  available  to  plot.  Only  the  response  of  the  IMU  unit 
to  45  degree  tilt  table  input  is  plotted.  However,  given  the  10  degree  per  second  pitch 
rate  of  the  tilt  table,  it  can  be  seen  that  the  system  accurately  recorded  each  attitude 
change  as  taking  4.5  seconds  to  complete.  The  slight  overshoots  following  each  motion 
may  indicate  that  the  scale  factor  for  the  y-  axis  angular  rate  sensor  is  slightly  high.  This 
effect  may  also  be  due  to  undersampling  problems.  The  flatness  of  the  curve  following 
stabilization  after  each  motion  indicates  that  a  reasonable  gain  value  has  been  determined 
as  does  the  distance  between  the  tails  of  each  step  [Ref.  11]. 

As  exemplified  by  Figure  4.2,  tilt  table  tests  of  the  IMU  unit  show  that  attitude 
sensing  is  achieved  to  an  accuracy  of  one  degree  or  better  under  demanding 
circumstances. 

2.  Magnetic  Compass  Calibration 

To- obtain  the  heading  information  in  the  SANS,  the  angular  rate  sensor  is  used  as 
a  high  frequency  input  source,  and  the  Precision  Navigation  Electronic  Digital  Compass 
(model  TCM2)  is  used  as  low  frequency  data  source.  In  a  series  of  ground  vehicle  tests, 
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Figure  4.3  Compass  Heading  without  Correction  vs.  DGPS  Track 


heading(degrees) 


Figure  4.4  TCM2  Compass  Error 
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with  continuous  GPS  update  (solid  line).  It  can  be  seen,  particularly  during  the  north- 
south  run,  that  the  filter  was  not  receiving  accurate  directional  information. 

The  investigation  of  compass  error  focused  on  three  areas:  possible  interference 
produced  by  the  golf  cart  electric  motor,  vibration,  and  error  of  the  compass  itself. 

The  tests  performed  by  Knapp  [Ref.  15]  showed  that,  golf  cart  electric  motor 
caused  interference,  but  its  magnitude  appeared  to  be  limited  to  approximately  half  of  a 
degree  and  could  be  compensated  for  with  an  appropriate  value  for  the  filter  gain.  It  was 
also  found  that  the  vibration  played  a  role  in  compass  error,  but  still  could  be  filtered  out 
with  an  appropriate  filter  gain. 

A  final  test  needed  to  determine  the  heading  dependent  compass  errors,  because 
of  a  different  TCM2  was  being  used  by  SANS  III.  The  TCM2  compass  has  a  self¬ 
calibration  [Ref.  3]  routine  which  is  designed  to  remove  the  effects  of  static  magnetic 
fields  caused  by  ferrous  materials  in  the  vicinity  of  the  compass.  The  calibration  routine 
is  not  capable  of  compensating  for  dynamic  magnetic  field  distortions.  From  the  ground 
vehicle  testing  results,  the  heading-dependent  compass  error  can  be  seen.  They  are 
especially  noticable  on  north-south  runs.  A  transit  (W.  and  L.E.  Gurley)  with  an 
accuracy  of  0.5  degrees  was  used  as  a  reference  to  determine  the  error  of  TCM2  compass 
that  was  caused  by  the  dynamic  magnetic  field  of  the  golf  cart.  The  TCM2  compass  was 
mounted  in  line  with  the  transit  on  the  golf  cart  facing  a  distant  object  with  a  known 
magnetic  bearing.  By  taking  this  object  as  a  starting  point,  the  compass  was  swung 
through,  the  entire  360  degrees,  taking  measurements  every  10  degrees  by  using  the 
opticaf  scale  of  the  transit.  A  comparison  was  made  between  the  two  indicated  headings, 
the  one  from  the  optical  scale  of  transit  and  the  reading  from  TCM2  compass.  Figure  4.4 
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optical  scale  of  the  transit.  A  comparison  was  made  between  the  two  indicated  headings, 
the  one  from  the  optical  scale  of  transit  and  the  reading  from  TCM2  compass.  Figure  4.4 
shows  the  difference  between  measurements  of  the  transit  and  those  of  the  TCM2 
compass.  Using  this  data,  a  new  lookup  table  and  a  linear  interpolating  function  were 
added  to  the  SANS  code  to  compensate  for  heading-dependent  errors. 

The  major  sensing  component  of  the  TCM2  compass  is  a,  set  of  magnetic  coils 
used  to  sense  the  magnetic  field  of  the  earth.  These  are  not  affected  by  acceleration  or 
inertia  as  is  the  "card"  in  a  mechanical  compass.  The  TCM2  does  use  a  "bubble  sensor" 
to  determine  the  attitude.  This  sensor  is  affected  by  linear  acceleration  to  some  degree. 
However,  the  only  purpose  of  the  TCM2  in  the  SANS  system  is  to  provide  drift 
correction  for  the  high  frequency  heading  information  obtained  from  the  rate-sensors  in 
the  IMU.  The  magnitude  and  duration  of  any  accelerations  experienced  in  SANS  system 
are  relatively  low  and  TCM2  data  is  low-pass  filtered.  Thus,  the  static  accuracy  of  the 
TCM2  compass  is  investigated  and  not  its  dynamic  accuracy  in  the  presence  of  extended 
large  magnitude  accelerations. 

Figure  4.5  shows  the  data  from  the  TCM2  after  compensation  using  the  table  data. 
C.  SYSTEM  TESTING  RESULTS 

1.  Simulation  and  Hardware  Bench  Testing  Results 
The  asynchronous  Kalman  filter  was  simulated  in  Matlab,  and  implemented  in 
SANS  III.  Simulation  and  hardware  bench  testing  results  are  presented  in  this 
subsection.  In  simulation,  asynchronism  is  coded  as  follows..  Every  20  to  30 
milliseconds,  the  filter  takes  measurements,  to  update  the  position  estimates  at  the  next 
(synchronous)  sample  time. 
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Figure  4.5  Compass  Heading  with  Correction  vs.  DGPS  Track 

In  hardware  testing,  the  system  is  fixed  on  a  laboratory  bench.  The  IMU, 
compass,  and  DGPS  are  operational.  Water  speed  information  is  taken  from  a  fixed 
voltage  source  in  place  of  the  water  speed  sensor. 

Simulation  results  are  shown  in  Figures  4.6  and  4.7.  The  simulation  is  designed 
to  simulate  the  hardware  system  in  the  stationary  bench  testing  condition.  Thus  the 
actual  position  of  the  system  is  fixed  at  (0,0).  It  is  seen  in  Figure  4.6  that  the  accuracy  of 
position  estimation  is  about  15  feet.  In  Figure  4.7,  a  north  water  speed  of  10.0 
feet/second  was  entered.  Since  the  system  was  stationary,  the  filter  was  able  to  estimate 
that  there  was  a  north  current  at  about  -10.0  feet/second  as  shown  in  Figure  4.7.  The 
estimated  east  current  stayed  zero,  as  expected. 
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Figure  4.6  Simulation  Result:  Estimated  North  Position 
Versus  Estimated  East  Position 


time(seconds) 

Figure  4.7  Simulation  Result:  Estimated  North  (lower  curve) 
and  East  Current  (upper  curve) 
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estimated  east  position(feet) 


Figure  4.8  Hardware  Bench  Test  Result:  Estimated  North  Position  vs. 

Estimated  East  Position 


Figure  4.9  Hardware  Bench  Test  Result:  Estimated  North  (lower  curve) 
And  East  Currents(upper  curve) 
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Examples  of  hardware  bench  testing  results  are  shown  in  Figures  4.8  and  4.9.  In 
Figure  4.8,  it  is  especially  interesting  to  note  that  there  is  a  long  straightline  segment 
starting  from  (0. 2,0.0)  ending  at  (0.0,10.0).  This  is  exactly  the  estimated  motion 
trajectory  of  the  system  during  the  first  second  of  operation.  During  this  time  period,  the 
filter  relied  entirely  on  inertial  data.  Since  the  water  speed  sensor  indicated  a  water  speed 
of  10.0  feet/second  in  the  north  direction,  the  filter  estimated  a  northerly  motion.  As  soon 
as  a  GPS  fix  was  obtained,  the  filter  updated  position  and  current  estimates.  After  about 
20  seconds,  the  estimated  north  current  converges  to  10.0  feet/second. 

Matlab  simulation  code  for  Kalman  filtering  is  presented  in  Appendix  B. 

2.  Ground  Vehicle  Testing  Results 

In  addition  to  the  bench  testing,  a  series  of  ground  vehicle  tests  were  performed  in 
preparation  for  at-sea  testing.  The  SANS  III  system  was  placed  on  a  golf  cart  (Figure 
4.10)  and  was  driven  along  a  predetermined  path.  Most  of  the  tests  were  conducted 
around  a  surveyed  course  in  the  parking  lot  next  to  the  Mechanical  Engineering 
Auditorium  at  Naval  Postgraduate  School.  Some  additional  tests  were  also  performed  in 
the  parking  lot  at  the  Navy  Golf  Course  to  observe  the  behavior  of  the  system  in  a 
different  location. 

Tests  were  conducted  with  continuous  DGPS  and  without  DGPS.  The  path  of  the 
parking  lot  (at  school)  taken  with  continuous  DGPS  is  shown  in  Figure  4.11.  The  cart 
started  from  the  (0,0)  position  and  traveled  northward  (at  350  degrees)  for  about  450  feet, 
made  a-U--tum,  traveled  southward  248  feet  (at  160  degrees)  and  about  210  feet  (at  150 
degrees),  made  a  last  turn  to  westward  and  returned  to  the  starting  point  along  the  last  leg 


40 


north/south  direction(feet) 


Figure  4.11  Parking  Lot  Test  Track  (at  School) 
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east/west  run(feet) 


Figure  4.12  Parking  Lot  Track  with  Continuous  DGPS  vs.  without  DGPS 

(without  compass  correction) 


Figure  4.13  Parking  Lot  Track  with  Continuous  DGPS  vs.  without  DGPS 

(with  compass  correction) 


Figure  4.14  Speed  Data  Through  First  Runs 

newSpeed  =  n*measuredSpeed  +  (1  -n)*previousSpeed  (4.2) 

previousSpeed  =  newSpeed  (4.3) 

The  speed  value  was  also  scaled,  because  the  later  test  results  showed  that  the  calculated 
speed  value  from  the  circuit  output  was  slightly  higher  than  the  actual  value. 

These  corrections  improved  the  speed  data  significantly  (Figure  4.15). 


Figure  4.15  Speed  Data  After  Filtering 
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After  compensating  heading-dependent  compass  error  and  fixing  the  problems 
with  speed  sensor,  other  ground  vehicle  tests  were  performed.  These  results  are  depicted 
in  Figures  4. 1 6  and  4. 1 7.  They  were  taken  at  the  Navy  Golf  Course  parking  lot.  The  plus 
signs  represent  the  DGPS  measurements  of  the  vehicle  trajectory.  The  solid  line  is  the 
trajectory  of  the  vehicle  following  the  same  path  computed  by  the  SANS  III  filter  without 
using  DGPS  data  during  the  entire  run.  Taking  the  DGPS  data  asihe  reference,  it  is  seen 
that  the  result  of  the  SANS  III  without  DGPS  is  accurate  to  within  15  feet  throughout  the 
run. 

D.  OBSERVATIONS 

The  following  observations  were  made  while  conducting  these  tests.  It  is 
necessary  to  receive  a  proper  DGPS  signal  to  run  the  SANS  software.  If  a  valid 
differential  correction  is  received  about  every  five  seconds,  the  SANS  program  will  be 
able  to  be  run  in  DGPS  mode.  It  took  a  considerably  long  time  to  get  a  valid  DGPS 
signal  for  ground  vehicle  testing.  These  are  several  things  that  could  be  the  reason  for 
not  getting  a  proper  signal. 

One  of  the  reasons  could  be  the  differential  antenna.  It  was  replaced  with  a  more 
powerful  one  to  eliminate  this  possibility. 

The  signal  itself  could  have  been  the  cause  the  problem.  Two  ground  stations  in 
Monterey  Bay  area  are  transmitting  DGPS  signals  for  our  specially  designed  unit  at  a 
certain  frequency.  If  the  testing  area  is  open  to  both  stations,  the  differential  unit 
sometimes  receives  both  signals  back  to  back  which  causes  the  program  not  to  run  in 
DGPS  mode.  The  signal  was  tested  with  a  hand  receiver,  and  it  was  hypothesized  that 
there  is  a  synchronization  problem  between  the  ground  stations.  Most  of  the  time,  the 
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east/wsst  run(feet) 


Figure  4.16  Continuous  DGPS  vs.  without  DGPS  Track 
(without  compass  correction) 


east/west  run(feet) 


Figure  4.17  Continuous  DGPS  vs.  without  DGPS  Track 
(with  compass  correction) 
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signal  was  reasonably  good  and  strong  enough  to  use. 

Eliminating  these  two  possible  problems  didn’t  solve  the  problem  with  the  DGPS 
signal.  It  turned  out  that  when  the  computer  of  SANS  was  turned  on,  the  DGPS  signal 
went  away.  The  SANS  computer  interfered  with  the  DGPS  signal.  Everything  on  the 
golf  cart  was  grounded  to  eliminate  the  noise  causing  interference  and  a  metal  shield  for 
the  SANS  computer  was  fabricated  to  eliminate  any  frequency  related  interference.  This 
helped,  but  didn't  solve  the  problem  completely. 

E.  CONCLUSIONS  AND  SUMMARY 

The  purpose  of  this  chapter  was  to  present  the  latest  testing  results  of  SANS  III 
and  explain  the  calibration  procedures  for  the  current  hardware  components.  These 
results  showed  that  the  Crossbow  IMU  had  an  accuracy  of  one  degree  with  proper  gains 
and  scale  factors,  and  the  TCM2,  after  heading  dependent  error  correction,  could  supply 
information  with  an  accuracy  of  one  degree. 

The  simulation  and  the  hardware  bench  test  results  demonstrated  that  the  SANS 
Kalman  filter  was  working  properly.  The  ground  vehicle  tests  proved  that  the  overall 
SANS  III  was  able  to  navigate  within  ±  15  feet  of  Global  Positioning  track  with  no 
Global  Positioning  update  for  three  minutes. 
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V.  CONCLUSIONS 


A.  SUMMARY 

The  purpose  of  this  thesis  was  to  improve  the  navigation  accuracy  of  the  latest 
version  of  SANS  by  properly  tuning  Kalman  filter  gains.  It  also  presents  the  test  and 
calibration  results. 

The  research  objectives  of  this  thesis  were:  (1)  to  implement  the  asynchronous 
Kalman  filter  within  the  SANS  system,  (2)  to  tune  filter  gains  by  comparing  the 
simulation  and  real  hardware  bench  test  results  and,  (3)  to  calibrate  the  components  and 
test  the  overall  system. 

The  objective  of  the  SANS  system  is  to  demonstrate  the  feasibility  of  using  low- 
cost,  small  components  to  navigate  inertially  between  DGPS  fixes.  To  achieve  the  first 
research  objective,  the  previous  SANS  constant-gain  filter  was  replaced  by  an 
asynchronous  Kalman  filter,  which  has  six  states  for  orientation  estimation  (still  constant 
gain),  and  eight  states  for  position  estimation.  The  asynchronous  nature  of  DGPS 
measurements  due  to  asynchronous  reacquisition  time  of  satellite  signals  and 
asynchronous  submergence  and  surfacing  of  the  AUV,  made  the  selection  of  an 
asynchronous  Kalman  filter  algorithm  a  logical  choice.  The  results  in  Chapter  4 
confirmed  that  using  a  Kalman  filter  improved  the  accuracy  of  the  system  significantly. 

Bench  test  results  indicated  that  the  newly  designed  system  provided  a  higher 
level  of  performance  than  the  previous  versions  of  SANS.  Examination  of  the 
experimental  data  indicated  that  the  new  IMU  used  in  this  research  was  capable  of 
meeting  all  SANS  requirements.  The  new  data  acquisition  and  processing  unit  increased 
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the  speed,  reliability,  and  compatibility  of  the  system.  Testing  the  new  asynchronous 
Kalman  filter  with  different  speed  and  heading  data  indicated  that  the  new  navigation 
filter  worked  properly. 

Ground  vehicle  tests  demonstrated  that  the  SANS  III  was  able  to  navigate  with  an 
accuracy  of  ±15  feet.  The  current  effort  was  directed  toward  at-sea  testing. 

B.  FUTURE  RESEARCH 

The  current  hardware  components  seem  to  be  working  at  a  very  reasonable 
sampling  rate,  but  technology  advances,  software  development,  and  the  amount  of 
research  put  into  test  and  evaluation  showed  that  the  future  of  SANS  will  be  subject  to 
many  changes.  New  SANS  components  should  be  chosen  to  reduce  the  size,  while 
improving  performance  and  decreasing  cost. 

For  the  near  future,  the  system  is  ready  for  at-sea  testing.  In  order  to  acquire  more 
accurate  navigation  information,  the  interference  between  the  SANS  computer  and  the 
DGPS  unit  should  be  solved.  For  future  versions,  it  is  suggested  that  the  DGPS  unit 
should  be  replaced  with  a  new  one  which  utilizes  the  broadcast  Coast  Guard  signal. 

The  performance  of  the  new  water  speed  sensor  should  be  tested.  The  accuracy 
of  the  sensor  seems  acceptable  for  the  SANS  objective.  It  might  need  calibration  prior 
to  at-sea  testing. 

The  asynchronous  Kalman  filter  has  been  implemented  as  a  navigation  filter.  The 
filter  gains  were  adjusted  during  the  test  and  calibration  period.  While  conducting  at-sea 
testing,  the  filter  constants  and  gains  might  need  to  be  adjusted  further. 
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During  the  ground  vehicle  testing,  it  appeared  that  magnetic  compasses  are  very 
sensitive  to  environmental  changes  and  hard  to  work  with.  Today’s  laser  gyros  are 
getting  smaller  and  cheaper.  They  are  more  robust  than  a  magnetic  compass.  These 
should  be  considered  for  the  future  versions  of  SANS. 

Adding  a  local  area  network  card  to  the  system  would  improve  SANS  portability 
and  could  potentially  change  the  way  the  sensors  are  integrated  and  utilized  for 
applications  other  than  AUVs. 
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APPENDIX  A.  MODIFIED  PART  OF  NAVIGATION  CODE (C++) 


A.  INS.H 

#ifndef  __INS_H 
#def ine  _INS_H 
#include  <time.h> 

#include  <math.h> 

#include  <dos.h> 

#include  <stdio.h> 

#include  <conio.h>  _ 

♦include  <fstream.h> 

♦include  <iostream.h> 

♦include  <assert.h> 

♦include  "toetypes .  h" 

♦include  "globals . h" 

♦include  "sampler . h" 

♦include  "MatrixCls . h" 

/***************************************************************** 

CLASS:  insClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Kadir  Akyol,  Suat  Arslan 

DATE:  11  July  1995  last  modified  March  2000 

FUNCTION:  Takes  in  linear  accelerations,  angular  rates,  speed  and 

heading  information  and  uses  Kalman  filtering  techniques  to  return  a 
dead  reconing  position. 

****************************  *********  ******************  ***********/ 
class  insClass  { 
public : 

insClass ();  //  Constructor,  initializes  gains 

-insClass {)  {}  //  destructor 

Boolean  insPosition (stampedSample& ) ;  //  returns  ins  estimated 
position 

//  Updates  the  x,  y  and  z  of  the  vehicle  posture 
void  correctPosition ( stampedS ample & ,  double); 

//  Sets  posture  to  the  origin  and  develops  initial  biases 
void  insSetUp (double,  stampedSampleS) ; 

private : 

file  for  filter  data 
ofstream  kalmanData; 

Matrix  h,  h_transpose,  p,  p_minus,  rl,  kl,  k,  x_hatMinus,  x_hat, 
z,  i,  phi, phi_transpose,  q,  hi,  hl__transpose,  k2,  r2,  k3,  z3, 
zMat ; 
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7/  ins  estimated  posture  (x  y  z  phi  theta  psi) 
float  posture [6] ; 

//  ins  estimated  linear  and  angular  velocities 
double  velocities [ 6] ; 

//  time  of  last  gps  position  fix 
float  las tGPS time; 

//  Accumulates  deltaT  between  GPS  data 
double  gpstimeCounter; 

//accumulates  deltaT 
double  cumtime; 

//  filter  time  constant 
int  tau; 

samplerClass  saml;  //  sampler  instance 

matrix  rotationMatrix;  //  body  to  euler  transformation  matrix 

float  current [3];  //  ins  estimated  error  current 

//  Software  bias  corrections  for  IMU  rate  sensors 
double  biasCorrection [3] ; 

//  Complementary  filter  gains  for  orientation, 
float  Konel,  Kone2,  Ktwo,  speed; 

//  Kalman  Filter  constants 

double  tau_l,  tau__2,  tau_3,  Dl,  D2,  D3; 

//  Transforms  body  coords  to  earth  coords,  removes  gravity  comp, 
void  transf ormAccels  (doublet]); 

//  Transforms  water  speed  reading  to  x  and  y  components 
void  transformWaterSpeed  (double,  doublet]); 

//  Tranforms  body  euler  rates  to  earth  euler  rates, 
void  transf ormBodyRates  (doublet]); 

//  Euler  integrates  the  accelerations  and  updates  the  velocities 
void  updateVelocities  (stampedSample&) ; 

//  Euler  integrates  the  velocities  and  updates  the  posture 
void  updatePosture  ( stampedSample& ) ; 

//  Builds  the  body  to  euler  rate  matrix 
matrix  buildBodyRateMatrix ( ) ; 

7/  Builds  the  body  to  earth  rotation  matrix 
void  buildRotationMatrix ( ) ; 

//  Calculates  the  imu  bias  correction  during  set  up 
void  calculateBiasCorrections (stampedSample&) ; 
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//  Applies  bias  corrections  to  a  sample 
void  applyBiasCorrections ( stampedS ample & ) ; 

//  Reads  filter  constants  from  'ins.cfg' 
void  readlnsConf igFile ( ) ; 

//constructs  h(4*8)  matrix 
void  constructHmatrix ( ) ; 

//constructs  P_minus(8*8)  matrix 
void  constructPminusMatrix ( )  ; 

//constructs  r(4*4)  matrix 
void  constructRlmatrix ( ) ; 

.//constructs  h(2*8)  matrix  (h  matrix  without  GPS) 
void  constructHlmatrix ( ) ; 

//constructs  r(2*2)  matrix  (r  matrix  without  GPS) 
void  constructR2matrix ( ) ; 

//constructs  phi (8*8)  matrix 

void  const ruct PhiMatrix (stampedSampl e&) ; 

//constructs  q(8*8)  matrix 

void  constructqMatrix ( stampedSample& ) ; 

} ; 

//  Post  multiply  a  matrix  times  a  vector  and  return  result, 
vector  operator*  (matrix&,  doublet]); 

#endif 
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B.  INS.CPP 

#include  <iostream.h> 

#include  <signal . h> 

#include  <assert.h> 

#include  <math.h> 

#include  "ins . h" 

#include  "matrix . h" 

#define  SIGFPE  8  //  Floating  point  exception 

/**************************************************** **  ************ 
PROGRAM:  insClass  ( constructor ) 

AUTHOR:  Eric  Bachmann,  Dave  Gay, Rick  Roberts,  Kadir  Akyol, 

Suat  Arslan 

DATE:  11  July  1995  last  modified  March  2000 

FUNCTION:  Constructor  initializes  kalman  filter  gains  and  linear 
and  angular  velocities 
RETURNS:  nothing 

CALLED  BY:  navigator  class 
CALLS :  none 

******************************************************************/ 
#ifndef  _NO_NAMESPACE 
using  namespace  std; 
using  namespace  math; 

#def ine  STD  std 
#else 

#def ine  STD 
#endif 

#ifndef  _NO_TEMPLATE 
typedef  matrix<double>  Matrix; 

#else 

typedef  matrix  Matrix; 

#endif 

#ifndef  _NO_EXCEPTION 

#  define  TRYBEGIN ( )  try  { 

#  define  CATCHERRORO  }  catch  (const  STD: : exception  e)  {  \ 

cerr  «  "Error:  "  .<<  e.what()  << 

endl ;  } 

#else 

#  define  TRYBEGIN ( ) 

#  define  CATCHERRORO 
#endif 

void  mylnverse (MatrixCls  &) ; 

insClass :: insClass (): h ( "h  matrix" , 4 , 8 ) , h_transpose ( "h  transpose",  8,4), 
p__minus("p  minus" ,  8 , 8 )  ,  rl  (  "rl  matrix",  4, 4)  y  kl("kl",  4,  4), 
k("k  matrix",  8,  4),  x_hatMinus  ( "x_hatmin" ,  8,  1), 
x_hat("x  hat”,  8,1),  i("unit  mat",  8,  8), 
phi_transpose ( "phitranspose" ,  8,  8) ,  hi ("hi", 2,8), 
hl_transpose ( "hi  transpose",  8,  2),  r2("r2  matrix" , 2 , 2 ) , 

k2("k2",  2,  2),  k3("k  mat  no  gps",  8,  2), 
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phi ("phi  matrix",  8,  8),  q("q  matrix",  8,  8), 
p("p  matrix",  8,  8),z3("z3  matrix" , 2 , 1 ) , 

zMat ( " zMat " , 4,1), kalmanData ( "xhat . dat") , gpstimeCounter (1.0) 

{ 

cerr  <<  "\nconstructing  insl"  «  endl; 


readlnsConfigFile ( ) ;  //  Read  the  config  file 

constructHmatrix ( ) ;  //constructs  4*8  h  matrix 


constructPminusMatrix ( ) ; 


//constructs  8*8  P  minus  matrix 


constructRlmatrix ( ) ; 
constructHlmatrix ( ) ; 
constructR2matrix ( ) ; 


//constructs  4*J  R1  matrix 

//constructs  2*8  h  matrix 
//constructs  2*2  R2  matrix 


velocities [0]  =  0.0; 
velocities [1]  =  0.0; 
velocities [2 ]  =  0.0; 
velocities [3]  =  0.0; 
velocities [4]  =  0.0; 
velocities [5]  =  0.0; 


//  x  dot 
//  y  dot 
//  z  dot 
//  phi  dot 
//  theta  dot 
//  psi  dot 


} 


//  Set  posture  to  straight  and  level  at  the  origin 


posture [0]  =  0.0; 

// 

X 

posture [1]  =  0.0; 

// 

y 

posture  [2]  =  0.0; 

// 

z 

posture [3]  -  0.0; 

// 

phi 

posture[4]  =  0.0; 

// 

theta 

posture [5]  =  0.0; 

// 

psi 

cerr  <<  "\nins  construction  complete"  «  endl; 


/*********************************************  ********************* 
PROGRAM:  insPosit 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Kadir  Akyol,  Suat  Arslan 

DATE:  11  July  1995  last  modified  March  2000 

FUNCTION : Make  dead  reckoning  position  estimation  using  kalman 
filtering. Inputs  are  linear  accelerations,  angular  rates,  speed  and 
heading. Primary  input  data  is  obtained  from  a  sampler  object  via  the 
getSample  method.  This  data  is  stored  in  the  sample  filed  of  a 
stampedSample  structure  called  newSample.  The  sample  field  is  then 
used  as  a  working  variable  as  the  linear  accelerations  and  angular 
rates  it  contains  are  converted  to  earth  coordinates  and  integrated 
to  determine  current  velocities  and  posture.  The  data  is 
asynchronous  kalman  filtered  against  itself,  speed  and  magnetic 
heading-. 

RETURNS:  position  in  grid  coordinates  as  estimated  by  the  INS 

CALLED  BY:  navPosit  (nav.cpp) 

CALLS:  getSample  ( sampler . cpp) 

findDeltaT  (ins . cpp) 

transformBodyRates  (ins. cpp) 
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buildRot ationMat rix  ( ins . cpp ) 
transf ormAccels  (ins) 
transf ormWaterSpeed  ( ins ) 


void  fpelnsPosit (int  sig) 

{if  (sig  ~  SIGFPE)  cerr  «  "floating  point  error  in  insPosit \n" ; } 

Boolean  insClass : : insPosition ( stampedSample&  newSample) 

{ 

signal  (SIGFPE,  fpelnsPosit) ; 

//  Working  variables 

double  thetaA,  phiA,  xlncline,  ylncline;  _ 

if  (saml . getSample (newSample)  )  { 

applyBiasCorrections (newSample) ; 

newSample . rawSample [0]  =  newSample . sample [ 0] ; 
newSample. rawSample [1]  =  newSample . sample [1] ; 
newSample. rawSample [2]  *  newSample . sample [2 ] ; 
newSample. rawSample [3]  =  newSample . sample [3] ; 
newSample. rawSample [4]  =  newSample . sample [4 ] ; 
newSample . rawSample [5]  =  newSample. sample [5] ; 
newSample. rawSample [6]  =  newSample . sample [ 6] ; 

newSample. rawSample [7]  =  newSample . sample [ 7 ] ; 

gpsTimeCounter+=newSample . deltaT; 
cumtime+=newSample . deltaT; 

xlncline  =  newSample . sample [0]  /  GRAVITY; 
ylncline  =  (newSample . sample [ 1 ]  - 

(newSample. sample [5]  *  newSample . sample [ 6] ) ) 

/  (GRAVITY  *  cos (posture [4] )) ; 

if  (fabs (ylncline)  >  1.0  ||  fabs (xincline) >1 . 0)  { 

static  int  inclineCount ( 0 ) ; 
gotoxy (1,24); 

cerr  «  "Inclination  errors:  "  «  ++inclineCount  «  endl; 
return  FALSE; 

} 

//  Calculate  low  freq  pitch  and  roll 
thetaA  =  asin (xlncline ) ; 
phiA  =  -asin (ylncline)  ; 

//  Transform  body  rates  to  euler  rates, 
transf ormBodyRates (newSample . sample ) ; 

-^--//-Calculate  estimated  roll  rate  (phi-dot). 

velocities [3]  =  newSample . sample [3]  +  Konel  *  (phiA  - 
posture [3] ) ; 

//  Calculate  estimated  pitch  rate  (theta-dot), 
velocities [4]  =  newSample . sample [ 4 ]  +  Kone2  *  (thetaA- 
posture [ 4 ] ) ; 
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//  Calculate  estimated  heading  rate  (psi-dot) . 
velocities [5]  = 

newSample . sample [5]  +  Ktwo  *  (newSample . sample [7]  - 
posture  [5] ) ; 

//  integrate  estimated  angular  rates  to  obtain  angles 
//  pitch  rate  to  angle 

posture [3]  +=  newSample . deltaT  *  velocities [ 3] ; 

//  roll  rate  to  angle 

posture  [4]  +==  newSample  .  deltaT  *  velocities  [ 4  ]  ; 

//  yaw  rate  to  angle 

posture [5]  +=  newSample . deltaT  *  velocities [5] ; 

//  constructs  Phi  matrix 
constructPhiMatrix (newSample) ; 

//constructs  Q  matrix  (8*8) 
constructqMatrix (newSample ) ; 

//calculate  x_hatMinus 
x_hatMinus  =  (  phi  *  x__ha t  )  ; 

//calculate  phi_transpo se 
phi . transpose (phi_transpose) ; 

//calculate  P_minus 

p_minus  =  ( ( (  phi  *  p  )  *  phi_transpose  )  +  q  ) ; 

//  Beginning  Kalman  Filter  loops 

if  (newSample . gpsFlag  & &’  gpsTimeCounter>=l . 0 )  { 

zMat . copy (0, 0,  (newSample . sample [6]  *  cos  (posture [5] ) ) ) 
zMat . copy (1, 0, (newSample . sample [6]  *  sin  (posture [5] ) ) ) 
zMat . copy (2, 0, newSample . est . x) ; 
zMat . copy (3, 0, newSample . est . y) ; 

//transpose  of  matrix  h 

h. transpose (h_transpose ) ; 

kl  =  ( ( (h*p_minus ) *h_transpose) +rl ) ; 

//take  inverse  of  matrix  kl 
myinverse (kl)  ; 

//calculate  matrix  Kalman  gain 
k  =  ( (p_minus  *  h_transpose ) *  kl) ; 

//calculate  x_hat 

x__hat  =  (  .x__hatMinus  +  (k  *  (zMat  -  (h  *  x_hatMinus )  )  )  ) 

"•//calculate  I  matrix 
i  =  i.unitMatrix  (8); 

//calculate  P  matrix 
p=  ((i-  (k*h))  *  p_minus); 
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//  Writing  filter  output  to  file 
kalmanData  «  cumtime  «  1  1 

«  newSample.gpsFlag  <<  1  1 

«  x_hat . getElement ( 0, 0)  <<  f  ' 

«  x_hat . getElement ( 1,  0)  «  1  ’ 

<<  x_hat . getElement (2, 0)  «  1  ' 

«  x_hat . getElement (3, 0)  «  ?  1 

«  x_hat . getElement (4 , 0)  «  1  1 

«  x_hat . getElement (5, 0)  «  1  1 

«  x_hat . getElement ( 6, 0 )  «  1  1 

«  x_hat .getElement (7, 0) 

<<  endl; 

newSample.gpsFlag  =  FALSE; 
gpsTimeCounter  =  0.0; 


}  . 
else  { 

z3 . copy (0,0,  (newSample. sample [6]  *  cos  (posture [5] )) ) 
z3. copy (1,0,  (newSample. sample [6]  *  sin  (posture [5] )) ) 

//hi  is  the  h  matrix  without  GPS 
hi . transpose (hl_transpose)  ; 

k2  =  ( ( (hl*p_minus) *hl_transpose) +r2) ; 

//  take  inverse  of  matrix  k2 
myinverse ( k2 ) ; 

//kalman  gain  matrix  without  gps 
k3  =  ( (pjminus  *  hl_transpo se)*  k2)  ; 

x_-hat  =  (  x_hatMinus  +  (k3  *  (z3  -  (hi  *  x_hatMinus)  )  )  )  ; 

//calculate  I  matrix 
i  =  i . unitMatrix  (8); 

//calculate  P  matrix 
p  =  ( (i  -  (k3  *  hi))  *  p_minus);  } 

//  Writing  filter ' output  to  file 
kalmanData  <<  cumtime  <<  T  T 

«  newSample.gpsFlag  «  '  1 

«  x_hat . getElement (0, 0)  «  *  1 

«  x_hat. getElement (1, 0)  «  '  1 

«  x_hat . getElement (2, 0)  «  T  ' 

«  x_hat . getElement (3, 0)  «  f  ' 

<<  x_hat .getElement (4, 0)  <<  '  1 

«  x_hat .getElement (5, 0)  <<  f  T 

«  x_hat . getElement ( 6, 0 )  «  1  1 

«  x_hat .getElement (7, 0) 

-  «  endl; 

} 

//  estimated  north  and  east  positions 
posture [0]  =  x_hat . getElement ( 6, 0 ) ; 
posture [1]  =  x_hat .getElement (7,0); 
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//  estimated  current  values 
newSample . sample [0]  =  posture [0]  ; 

newSample . sample [ 1]  =  posture [1]  ; 
newSample . sample [2]  =  posture [2]  ; 
newSample . sample [3]  =  posture [3]; 
newSample. sample [4]  =  posture [4]; 
newSample. sample [5]  =  posture [5] ; 
return  TRUE; 

} 

else  { 

return  FALSE;  //  New  I MU  information  was  unavailable. 

} 

} 

/**********  ******************************************************** 
PROGRAM:  insSetUp 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Initializes  the  INS  system.  Sets  the  posture  to 

the  origin . Initializes  the  heading  using  magnetic  compass 
information.  Initializes  the  last  GPS  fix  and  last  IMU  information 
times . 

RETURNS:  void 

CALLED  BY:  initializeNavigator  (nav) 

CALLS:  calculateBiasCorrections  (ins) 

get Sample  (sampler) 
buildRotationMatrix  (ins) 
transformWaterSpeed  (ins) 

*******************★********************************************'*■*'/ 
void  fpel-nsSetUp  (int  sig) 

{if  (sig  =-  SIGFPE)  cerr  «  "floating  point  error  in  inSetUp\n";} 

void  insClass :: insSetUp (double  originTime,  stampedSample&  posit) 

{ 

cerr  <<  "  Initializing  INS."  <<  endl; 
signal  (SIGFPE,  fpelnsSetUp) ; 

saml . initSampler  ( ) ;  //  Initialize  the  sampler 

saml . getSample (posit) ; 

cerr  «  "  X  accel  =  "  <<  posit . sample [ 0] 

«  ",  Y  accel  =  "  «  posit . sample [ 1 ] 

«  ",  Z  accel  =  "  «  posit . sample [2]  «  endl; 

calculateBiasCorrections (posit ) ;  //  set  imu  biases 

posture [5]  =  posit . sample [7 ] ;  //set  initial  true  heading 

buildRotationMatrix () ;  //set  initial  speed 

transformWaterSpeed (posit. sample [6] ,  velocities) ; 

posit . current [0]  =  current [0]; 
posit .current [1]  =  current [1]; 
posit . current [2]  =  current [2] ; 
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laStGPStime  =  originTime; 


//  initialize  times 


cerr  «  "  INS  initialization  complete.”  «  endl; 

} 

/********************************************************** 

PROGRAM :  t rans f ormAccels 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Transforms  linear  accelerations  from  body  coordinates 

to  earth  coordinates  and  removes  the  gravity  component  in  the  z 
direction.  _  . 

RETURNS :  void 

CALLED  BY:  navPosit 

CALLS :  none 

**********************************************************/ 
void  insClass : :transf ormAccels  (double  newS ample [] ) 

{ 

vector  earthAccels ; 


newSample [ 0]  -=  GRAVITY  *  sin (posture [ 4 ])  ; 

newSample[l]  +=  GRAVITY  *  sin (posture [3] )  *  cos (posture [ 4 ])  ; 
newSample [2]  +=  GRAVITY  *  cos (posture [3] )  *  cos (posture [ 4 ]) ; 

earthAccels  =  rotationMatrix  *  newSample; 

newSample [0]  =  earthAccels . element [ 0] ; 
newSample [1]  =  earthAccels . element [1] ; 
newSample [2]  =  earthAccels . element [2]  ; 


/ 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

coordinates 


transformWaterSpeed 
Eric  Bachmann,  Dave  Gay 
11  July  1995 

Transforms  water  speed  into  a  vector  in  earth 
and  returns  them  in  the  speedCorrection  variable. 


RETURNS:  void 

CALLED  BY:  navPosit 

CALLS :  none 

******************************************************************/ 


void  insClass: : transformWaterSpeed  (double  waterSpeed,  double 
speedCorrection [ ] ) 

{ 

double  water [3]  =  {waterSpeed,  0.0,  0.0}; 
vector  waterVelocities  =  rotationMatrix  *  water; 

spe'erdCorrection  [0]  =  waterVelocities . element  [ 0 ]  ; 
speedCorrection  [1]  =  waterVelocities . element [1] ; 
speedCorrection  [2]  =  waterVelocities . element [2 ] ; 

} 

/****************************************************************** 
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PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS : 


transf ormBodyRates 
Eric  Bachmann,  Dave  Gay 
11  July  1995 

Tranforms  body  euler  rates  to  earth  euler  rates 
none 

insPosit 

buildBodyRateMatrix 


void  insClass :: transf ormBodyRates  (double  newSample [ ] ) 

{ 

matrix  bodyRateMatrix  =  buildBodyRateMatrix (  ); 
vector  earthRates  =  bodyRateMatrix  *  &  (newSample  [3] K* 

newSample [3]  =  earthRates . element [0] ; 
newSample [4 ]  =  earthRates . element [ 1] ; 
newSample [5]  =  earthRates . element [2] ; 

} 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS : 


buildBodyRateMatrix 
Eric  Bachmann,  Dave  Gay 
11  July  1995 

Builds  body  to  Euler  rate  translation  matrix. 

rate  translation  matrix 

insPosit 

none 


matrix  insClass: : buildBodyRateMatrix ( ) 

{ 

matrix  rateTrans; 


float  tth  =  tan (posture [4]  ) , 
sphi  =  sin (posture [3] ) , 

cphi  =  cos (posture [3] )  , 
cth  =  cos (posture [4 ])  ; 


rateTrans . element [0] [0] 
rateTrans . element [0] [1] 
rateTrans . element [0] [2] 
rateTrans . element [1] [0] 
rateTrans . element [1] [1] 
rateTrans . element [1] [2] 
rateTrans . element [2 ] [0] 
rateTrans . element [2] [1] 
rateTrans . element [2 ] [2] 


1.0; 

tth  *  sphi; 
tth  *  cphi; 
0.0; 
cphi  ; 

-sphi; 

0.0; 

sphi  /  cth; 
cphi  /  cth; 


return  rateTrans; 

PROGRAM:  buildRotationMatrix 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 
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FUNCTION: 
RET-URNS : 
CALLED  BY: 
CALLS': ' 


Sets  the  body  to  earth  coordinate  rotation  matrix. 

void 

insPosit,  insSetUp 

none 

*************************************************/ 


void  insClass: : buildRotationMatrix ( ) 


{ 

float  spsi  =  sin (posture [5] ) , 
cpsi  =  cos (posture [5] ) , 
sth  =  sin (posture [4 ]) , 
sphi  =  sin (posture [3] ) , 
cphi  =  cos (posture [3] ) , 
cth  =  cos (posture [ 4 ]) ; 

rotationMatrix. element [0] [0]  = 
rotationMatrix. element [0] [1]  - 
rotationMatrix. element [0] [2]  = 
rotationMatrix. element [1] [0]  = 
rotationMatrix. element [1] [1]  = 
rotationMatrix. element [1] [2]  = 
rotationMatrix. element [2] [0]  = 
rotationMatrix. element [2] [1]  = 
rotationMatrix. element [2] [2]  = 

} 


cpsi  *  cth; 

(cpsi  *  sth  *  sphi)  -  (spsi  *  cphi) ; 
(cpsi  *  sth  *  cphi)  +  (spsi  *  sphi); 
spsi  *  cth; 

(cpsi  *  cphi)  +  (spsi  *  sth  *  sphi); 
(spsi  *  sth  *  cphi)  -  (cpsi  *  sphi) ; 
-sth; 

cth  *  sphi; 
cth  *  cphi; 


/****************************************************************** 
PROGRAM:  postmultiplication  operator  * 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Post  multiply  a  3  X  3  matrix  times  a  3  X  1  vector  and 

return  the  result 
RETURNS:  3X1  vector 

CALLED  BY: 

CALLS:  Nonel 

******************************************************************/ 


vector  operator*  (matrix^  transform,  double  state []) 

{ 

vector  result; 

for  (int  i  =  0;  i  <  3;  i++)  { 

result . element [i]  =  0.0; 
for  (int  j  =  0;  j  <  3;  j++)  { 

result . element [i]  +=  transform. element [i] [j ]  *  state [j]; 

} 

}"  — 

return  result; 

} 

/****************************************************************** 
PROGRAM:  calculateBiasCorrections 
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AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 


FUNCTION:  Calculates  the  initial  imu  bias  by  averaging  a  number 

of  imu  readings . 


RETURNS:  none 

CALLED  BY:  insSetup 

CALLS :  none 


/ 


void  fpeCalculateBiasCorrections (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 
CalculateBiasCorrections\n"; } 


void  insClass: : calculateBiasCorrections (stampedSample&  biasSample) 
signal  (SIGFPE,  fpeCalculateBiasCorrections ) ; 
int  biasNumber (tau/10) ; 

bias Correct ion [0]  =  0.0;  //  p  roll  rate 

biasCorrection [1]  -  0.0;  //  q  pitch  rate 

biasCorrect ion [ 2 ]  =  0.0;  //  r  yaw  rate 

for  (int  i  =  0;  i  <  biasNumber;  i++)  { 

//  Find  the  average  of  the  biasNumber  packets 
while (! saml . getSample (biasSample) )  {/*  */};  ■ 

//  roll-rate/b# 

biasCorrect ion  [ 0 ]  +=  biasSample . sample [3] /biasNumber ; 

//  -pitch-rate/b# 

biasCorrect ion [ 1 ]  +=  biasSample . sample [ 4 ] /biasNumber ; 

//  yaw-rate/b# 

biasCorrection  [2]  +=  biasSample . sample [5] /biasNumber; 


//  set  biasSample  correction  fields  to  new  bias  correction  values 
//  negative  biasCorrection  value  is  taken  so  biases  are  added  to 
//  sensor  values 

biasSample  . bias  [  0 ]  =  biasCorrection  [  0 ]  —  *~(biasCorrection[0]); 
biasSample. bias [1]  =  biasCorrection [1]  =  -(biasCorrection[l]); 
biasSample .bias [2]  =  biasCorrection [2 ]  =  - (biasCorrection [2] ) ; 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS : 


applyBiasCorrections 

Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Applies  updated  bias  corrections  to  a  sample. 

void 

insPosit 

none 


void  insClass :: applyBiasCorrections (stampedSample&  posit) 
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const  float  sampleWght (posit . deltaT/tau) ; 
const  float  biasWght(l  -  sampleWght); 

//Calculate  updated  bias  values 

biasCorrection [ 0 ]  =  (biasWght  *  biasCorrection [ 0] ) 

-  (sampleWght  *  posit . sample [3] ) ; 

biasCorrection [1]  =  (biasWght  *  biasCorrection [ 1] ) 

-  (sampleWght  *  posit . sample [ 4 ]) ; 

biasCorrection [2]  =  (biasWght  *  biasCorrection [2] ) 

-  (sampleWght  *  posit . sample [5]); 


//Apply  the  bias  to  the  sample 
posit .sample [3]  +=  biasCorrection [0] ; 
posit . sample [ 4 ]  +=  biasCorrection [1] ; 

posit . sample  [5]  +=  biasCorrection [2]  ; 

//Save  the  bias  to  the  sample 
posit .bias [0]  =  biasCorrection [0] ; 
posit . bias [ 1 ]  —  biasCorrection [1] ; 
posit .bias [2]  =  biasCorrection [2]  ; 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 


**************************************************** 
readlnsConf igFile 

Rick  Roberts,  Eric  Bachmann,  Suat  Arslan 
02  Nov  96  last  modified  march  2000 
Reads  filter  constants  from  1  ins-,  cfg' 
void 


CALLED  BY: ins  class  constructor 


CALLS : 


none 


void  insCla ss: : readlnsConf igFile ( ) 

cerr  <<  "Reading  ins  configuration  file. 11  <<  endl; 
if stream  insCf gFile ( "ins . cf g" ,  ios::in). 


if  ( ! insCfgFile)  { 

cerr  «  "could  not  open  ins 


configuration  file!"  <<  endl; 


} 

else  { 

char  comment [128] ; 
insCfgFile 
»  Konel  »  comment 

»  Kone2  »  comment 
»  Ktwo  »  comment 
-  -  »  tau  1  »  comment 

»  tau~2  »  comment 
»  tau_3  »  comment 
»  D1  »  comment 
»  D2  »  comment 
»  D3  »  comment 
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>>  tau  >>  comment 
»  speed  »  comment 
»  current  [0]  >>  comment 
»  current [1]  »  comment 
»  current  [2]  »  comment; 

cout  <<  "\nKonel:  rf  <<  Konel  <<  TT\nKone2:  "  «  Kone2 
«  "\nKtwo:  "  <<  Ktwo  <<  "\ntau_l:  "  «  tau_l 
«  "\ntau_2:  "  «  tau__2  «  "\ntauJ3:  "  «  tau_3 
«  M\nDl:  "  «  D1  «  ,f\nD2:  11  «  D2 
«  "\nD3:  "  <<  D3  «  ,r\ntau:  "  «  tau 
«  f,\nx  Current:  "  «  current  [0]  «  f,\ny  Current:  " 

<<  current  [1]  <<  "\nz  Current:  "«  current  [2]  «  endl; 

} 

ins.Cf gFile .  close  (  )  ; 

} 

PROGRAM :  const ructHmatrix ( ) 

AUTHOR:  Kadir  Akyol 

DATE:  01  March  1999 

FUNCTION:  constructs  h  matrix 
RETURNS :  none 

CALLED  BY : ins  class  constructor 
CALLS :  none 

*****************************************************************/ 


void  fpeGonstructHmatrix (int  sig) 

{if  (sig  ==  SIGFPE )  cerr  <<  "floating  point  error  in 
constructHmatrix\nT? ;  } 

void  insClass: : constructHmatrix ( ) 

{ 

signal  (SIGFPE,  fpeconstructHmatrix)  ; 

h. copy (0, 0,1.0); 
h. copy (1, 1,1.0); 
h. copy (2,  4,1.0)  ; 
h .  copy  (2,  6,1.0); 
h . copy ( 3, 5 , 1 . 0 ) ; 
h. copy ( 3, 7 , 1 . 0 )  ; 

return  ; 

}//end  constructHmatrix ( ) 


/****+***+******************+************************************** 
PROGRAM:  const ructPminusMatrix ( ) 

AUTHOR:  Kadir  Akyol,  Suat  Arslan 

DATE:  01  March  1999  last  modified  march  2000 

FUNCTION:  constructs  Pjninus  matrix 

RETURNS :  none 

CALLED  BY:  ins  class  constructor 
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CALLS :  none 


void  fpeconstructPminusMatrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 
constructPminusMatrix\n" ; } 


void  insClass: : construct PminusMatrix ( ) 

{ 

signal  (SIGFPE,  fpeconstructPminusMatrix)  ; 


p_minus . copy ( 0 , 0 , 0 . 1 ) ; 
p  jninus . copy ( 1 , 1 , 0 . 1 )  ; 
p_minus . copy ( 2 , 2 , 0 . 1 ) ; 
p_minus . copy ( 3 , 3 ,  0 . 1 )  ; 
p  jninus . copy ( 4 , 4 , 0 . 1 )  ; 
p  jninus . copy ( 5 , 5 , 0 . 1 )  ; 
p  jninus  .  copy  ( 6 ,  6 ,  0 . 1 )  ; 
pjninus . copy ( 7 , 7 , 0 . 1 ) ; 


return  ; 

}//end  constructPminusMatrix ( ) 


PROGRAM :  constructPMat rix ( ) 
AUTHOR:  Suat  ARSLAN 

DATE:  28  May  1999 


FUNCTION:  constructs  P  matrix 

RETURNS :  none 

CALLED  BY:  ins 


CALLS:-  None 


void  fpeconstructPMatrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 
constructPMatrix\n" ; } 

void  insClass: : constructPMatrix ( ) 

{ 

signal  (SIGFPE,  fpeconstructPMatrix) ; 

p . copy ( 0 , 0 , 0 . 5 ) ; 
p.copy (1,  1,0.5); 
p . copy (2, 2,1.0); 
p . copy (3, 3, 1 . 0 ) ; 
p. copy (4 , 4,10.0); 
p . copy (5,5,10.0) ; 
p . copy (6, 6, 15.0) ; 
p. copy (7, 7,15.0); 

return  ; 

}//end  constructPMatrix ( ) 

/****************************************************************** 
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PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION : 
RETURNS : 
CALLED  BY: 
CALLS : 


constructRlmatrix ( ) 

Kadir  Akyol,  Suat  Arslan 

01  March  1999  last  modified  march  2000 

constructs  rl  matrix 

none 

ins  class  constructor 

none 

*****************************  *************** 


/ 


void  fpeconstructRlmatrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 

constructRlmatrix\n"; } 


void  insClass : : constructRlmatrix ( ) 

signal  (SIGFPE,  fpeconstructRlmatrix); 


rl . copy (0, 0,0.01); 
rl. copy (1, 1,0.01); 
return  ; 

}//end  constructRlMatrix ( ) 


/*********************************^+^^^^^^^^^^ 
PROGRAM:  constructHlmatrix ( ) 

AUTHOR:  Kadir  Akyol 

DATE:  01  March  1999 

FUNCTION:  constructs  h  matrix 

RETURNS :  none 

CALLED  BY:  ins  class  constructor 
CALLS :  None 

**************************************^  +  ^^^^^^^  +  ^^/ 


void  fpeconstructHlmatrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point 
constructHlmatrix\n"; } 


error  in 


void  insClass: : constructHlmatrix ( ) 

{ 

signal  (SIGFPE,  fpeconstructHlmatrix); 

hi. copy (0, 0, 1.0) ; 
hi. copy(l, 1,1.0); 
return  ; 

} / / end  constructHlmatrix ( ) 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION : 
RETURNS : 
CALLED  BY: 
CALLS : 


constructR2matrix ( ) 

Kadir  Akyol,  Suat  Arslan 

01  March  1999  last  modified  march  2000 

constructs  r2  matrix 

none 

ins  class  constructor 
None 
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void  fpeconstructR2matrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  m 
constructR2matrix\n"; } 

void  insClass :  : constructR2matrix ( ) 

signal  (SIGFPE,  fpeconstructR2matrix) ; 

r2 . copy (0, 0,0.01); 
r2 . copy (0, 1,0.0) ; 
r2 . copy (1, 0,0.0); 
r2 . copy (1, 1,0.01); 

return  ; 


} / /end  constructR2matrix () 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS : 


constructPhiMatrix ( ) 

Kadir  Akyol,  Suat  Arslan 

01  March  1999  last  modified  march  2000 

constructs  phi  matrix 

none 

insPosit 


None 

********* 


* 


/ 


void  fpeconstructPhiMatrix (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 

cunstructPhiMatrix\n";  } 

void  insClass :: constructPhiMatrix (stampedSampleS  delta) 

signal  (SIGFPE,  fpeconstructPhiMatrix); 
double  xx,  yy,  zz; 

xx  =  -  ( delta. deltaT) /tau_l; 

xx  =  exp (xx) ; 

yy  =  -  (delta. deltaT) /tau_2; 
yy  =  exp (yy) ; 

zz  =  -  (delta. deltaT) /tau_3; 
zz  =  exp (zz) ; 

phi . copy ( 0 , 0 , xx ) ; 

phi . copy (1, 1, xx) ; 

phi. copy (2, 2, yy) ; 

phi . copy (3, 3, yy)  ; 

phi. copy (4, 4, zz)  ; 

phi' copy (5, 5,zz); 

phi. copy (6,0, ((l-xx)*tau_l)); 

phi  ."copy  (6, 2,  (  (1-yy)  *tau_2)  )  ; 

phi! copy (7, 1, ( (1-xx) *tau_l) ) ; 

phi. copy (7, 3, ( ( 1-yy) *tau_2) ) ; 
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return  ; 

}//end  constructPhiMatrix ( ) 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS : 


constructqMatrix ( ) 

Kadir  Akyol,  Suat  Arslan 
01  March  1999 
constructs  Q  matrix 
none 
ins Posit 
None 


void  fpeconstructqMatrix (int  sig) 

{if  (sig  ==  SIGFPE )  cerr  «  "floating  point  error  in 
cunstructqMatrix\n"; } 

void  insClass : : constructqMatrix (stampedSampleS  delt) 

signal  (SIGFPE,  fpeconstructqMatrix) ; 

double  uu,  ww,  vv, zz, yy,  Q661, Q662, Q60, Q62, Q71, Q73, Q66; 

uu  =  -(2.0  *  delt .deltaT) /tau_l; 

uu  =  exp ( uu )  ; 

ww  =  -(2.0  *  delt. deltaT ) /tau_2; 

ww  =  exp (ww) ; 

w  =  -(2.0  *  delt. deltaT ) /tau_3; 

vv  =  exp(vv); 

zz  =  -- (delt. deltaT ) /tau_l; 
zz  =  exp ( zz) ; 

yy  =  -(delt. deltaT) /tau_2; 
yy  =  exp(yy); 

Q661=D1* ( (tau_l/2) * (1-uu) -( (2*tau_l) * (1-zz) ) +delt . deltaT) ; 
Q662=D2* ( (tau_2/2) * (1-ww) - ( (2*tau  2) * (1-yy) ) + (delt . deltaT) ) ; 
Q66=Q661+Q662 ; 

Q60=D1* (0 . 5+ ( 0 . 5*uu) -zz) ; 

Q62=D2  * ( 0 . 5+ ( 0 . 5*ww) -yy) ; 

Q71=Q60; 

Q73=Q62; 

q. copy (0, 0, ( (uu) * (Dl/ (2. 0*tau_l) ) ) ) ; 
q. copy (1, 1, ( (uu) * (Dl/ (2.0*tau_l) ) ) ) ; 
q. copy (2, 2,  (  (ww)  * (D2/ (2. 0*tau_l) ) ) ) ; 
q. copy (3, 3, ( (ww) * (D2/ (2.0*tau_l) ))); 
q. copy (4,4, ( (vv) * (D3/ (2.0*tau_l) ))); 
q-.copy  (5,  5,  (  (vv)  *  (D3/ (2. 0*tau_l)  )  )  )  ; 
q. copy (6,6, (Q66 ) ) ; 
q. copy (7,7, (Q66) ) ; 
q. copy (0, 6, (Q60) ) ; 
q. copy (6, 0, (Q60) ) ; 
q. copy (2, 6, (Q62) ) ; 
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q. copy (6, 2, ( Q62 ) ) ; 
q. cdpy (1,7, (Q71) ) ; 
q. copy (7, 1, (Q71) ) ; 
q. copy (3, 7 , (Q73) ) ; 
q. copy (7, 3, (Q73) ) ; 


return  ; 


} //end  constructqMatrix ( ) 

/****************************************************************** 
PROGRAM:  mylnverse (MatrixCls&) 

AUTHOR:  Suat  ARSLAN  — 

DATE:  29  july  1999 

FUNCTION:  invert  the  matrix 

RETURNS :  none 

CALLED  BY:  ins 
CALLS :  None 


******************************* 
void  mylnverse (MatrixCls&  kl)  { 


/ 


Matrix  k3; 

k3 . SetSize (kl . getCol ( ) ,  kl.getRow() ) ; 
for  (int  i  =  0;  i  <  kl.getColO;  i++)  { 

for (int  j  =0;  j  <  kl . getCol ( ) ;  j++)  ( 

k3(i,  j)  =  kl . getElement (i,  j); 


} 


} 

k3  =  !k3; 


f or-( int  i  =  0;  i  <  kl.getColO;  i++) 
for  (int  j  =  0;  j  <  kl.getColO 
kl.copy(i,  j,  k3(i,  j)); 

} 


} 

}//  end  of  function 
//  end  ins.cpp 


j++)  { 
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C.  ATOD.H 

#define  _ATOD_H 

#include  <stdio.h> 
♦include  <f stream. h> 
♦include  <iostream.h> 
tinclude  <dos.h> 


♦define  ENABLED  1 
♦define  DISABLED  0 

♦define  INPUT  1 
♦define  OUTPUT  0 

//♦define  TRUE  1 
//♦define  FALSE  0 

♦define  STATUS_BYTE  0 
♦define  START_CONVERSION  0 
♦define  READ_DATA  1 
♦define  DACJJPDATE  1 
♦define  CLEAR_BOARD  2 
♦define  SCAN_BURST_SLCT  3 
♦define  CHANNEL_SLCT  5 
♦define  GAINJ3LCT  5 
♦define  IRQ_ENABLE  5 
♦define  EXT_TRIG_ENABLE  5 
♦define  PPI_A  4 
♦define  PPI_B  5 
♦define  PPI_C  6 
♦define  PPI_CTRL  7 
♦define  TIMER_A  8 
♦define  TIMER_B  9 
♦define  TIMER_C  10 
♦define  TIMER_CTRL  11 
♦define  DAC1_LSB  12 
♦define  DAC1_MSB  13 
♦define  DAC2_LSB  14 
♦define  DAC2_MSB  15 
♦define  C LE AR_I RQ_S TAT  14 
♦define  CLEAR_DMA_DONE  15 

♦define  PPI_PORT_A  0 
♦define  PPI_PORT_B  1 
♦define  PPI_PORT_C  2 

♦define  DISABLE_BOTH  0 
♦  define  'SCAN_ENABLE  1 
♦define  BURST_ENABLE  2 

♦define  SOURCE_AD_START  0 
♦define  SOURCE_DMA_DONE  1 
♦define  SOURCE  TRIGGER  2 


#def ine  SOURCE_PACER_CLOCK  3 
class  atodClass{ 
public: 

atodClass ( )  { }  ; 

-atodClass ( ) { } ; 

void  InitatodO; 

void  InitializeBoardSettings (unsigned  ,  float  ) 

float  DigitalToReal (int  DigitalValue) ; 

void  ResetBoard ( void) ; 

void  ClearBoard ( void) ; 

void  ClearlrqStat (void) ; 

void  ClearDmaDone (void) ; 

void  SetChannel (unsigned  char); 

void  SetGain (unsigned  char); 

void  SetIRQStatus (char) ; 

void  SetExternalTrigger (unsigned  char); 
void  SetlnterruptSource (unsigned  char); 
void  ScanBurstEnable (unsigned  char); 
void  SetScanChannels (int ) ; 
void  StartConversion ( void) ; 
int  ConversionDone ( ) ; 
int  ReadData (void) ; 

void  ClockMode (unsigned  char,  unsigned  char) ; 
void  ClockDivisor (unsigned  char,  unsigned  int) ; 
void  SetUserClock (float ) ; 

-  void  SetPacerClock (float) ; 

_ //Boolean  ClockDone (unsigned  char); 
unsigned  char  ReadDigitallO (unsigned  char) ; 
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void  WriteDigitallO (unsigned  char,  unsigned  char); 
void  Conf igurelOPorts (unsigned  char,  unsigned  char) 
void  UpdateDAC (unsigned  char,  float); 

} ; 

#endif 

//end  atod.h 
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D  .  ATOD  .  CPP 

#include  ."atod.h" 

unsigned  BaseAddress; 

float  VoltageRange, 

DACSlope, 

ConversionFactor, 

Baseline  ; 

int  DACOffset; 

void  atodClass : : Initatod ( ) 

{ 

InitializeBoardSettings (768, 10. 0)  ; 
ResetBoard ( ) ; 

ClearBoard ( ) ; 

SetChannel ( 1) ; 

SetExternalTrigger (DISABLED) ; 

SetGain ( 1 ) ; 


InitializeBoardSettings 

The  InitializeBoardSettings  procedure  is  used  to  set  the  Base  address 
variable  and  calculate  the  conversion  factor  for  converting  between 
digital  ' 

values  and  volts.  Since  the  base  address  variable  is  not  set  until 
this 

procedure  is  called,  make  certain  that  you  call  this  procedure  before 
any  others  in  this  file. 

********  **************************************************************/ 


void  atodClass :: InitializeBoardSettings (unsigned  BA,  float  Range) 

{ 

BaseAddress  =  BA; 

VoltageRange  =  Range; 

ConversionFactor  =  VoltageRange  /  4096.0; 

Baseline  =  0; 


The  Function  DigitalToReal  converts  a  digitized  value  to  a  real  world 
value.  In  these  sample  programs  the  conversion  factor  and  baseline 
correspond  to  converting  between  digitized  values  and  volts. 
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float  atodClass : : DigitalToReal (int  DigitalValue) 

{ 

return (DigitalValue  *  ConversionFactor  +  Baseline); 

} 

/***************  +  ***********  +  ******  +  **  +  ***  +  **  +  ******  +  ******  +  -/'  +  +  ******** 


ResetBoard 

The  ResetBoard  procedure  is  used  to  reset  the  DM5406.  The  8255  PPI  is 
configured  so  that  ports  A  and  C  are  input  and  port  B  is  output,  a 
dummy  A-D  conversion  is  performed  and  then  the  clear  board  command  is 
sent .  _ 


void  atodClass :: ResetBoard (void) 

{ 

outportb (BaseAddress  +  PPI_CTRL,  0x99);  /*  Set  PPI  Port  B  for 

output  */ 

outportb (BaseAddress  +  PPI_B,  0);  /*  Channel  0,  Gain  1, 

Ext  Trig  = 


outportb (BaseAddress 
outportb (BaseAddress 

outportb (BaseAddress 
conversion  */ 

delay (5) ; 

outportb (BaseAddress 


+  SCANJ3URSTJSLCT, 

+  CLEAR_BOARD ,  0)  ; 

+  START_CONVERS I ON ,  0 

+  CLEAR  BOARD,  0)  ; 


/*  Start  a  Dummy 


/*  Any  value  will  do  */ 


Disabled,  IRQ  =  Disabled  */ 

0); 

) ; 


} 


ClearBoard 

The  clear  board  procedure  writes  to  the  CLEAR  BOARD  port  on  the 
DM5406. 


void  atodClass :: ClearBoard (void) 

{ 

outportb (BaseAddress  +  CLEAR_BOARD,  0);  /*  Any  value  will  do  */ 

} 

Clear I rqS tat us 
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The  ClearlrqStat  procedure  reads  from  the  CLEAR_IRQ_STAT  port  on  the 
DM5406'; 

********* ******************************************************** *****/ 


void  atodClass : : ClearlrqStat (void) 

{ 

inportb  (BaseAddress  +  CLEAR_IRQ_STAT)  ; 

} 

/********************************************************************** 

ClearDmaDone 

The  ClearDmaDone  procedure  reads  from  the  CLEAR_DMA_ONE  port  on  the 
DM5406. 

**********************************************************************/ 
void  atodClass: : ClearDmaDone (void) 

{ 

inportb  (BaseAddress  +  CLEAR_DMA_JDONE )  ; 

} 

/********  ************************************************************** 
SetChannel 

The  SetChannel  procedure  is  used  to  set  the  channel  bits,  B0..B3,  in 
the  CHANNEL  SELECT  register  (BA  +5).  Note  how  this  procedure  sets 
only 

the  channel  select  bits;  it  does  not  change  the  other  bits  in  this 
register. 

This  is  important  because  if  you  unintentionally  clear  the  other  bits 
it 

can  cause  unexpected  behavior  of  the  DM5406. 
**********************************************************************/ 


void  atodClass :: SetChannel (unsigned  char  ChannelNumber ) 

{ 

unsigned  char  B; 


B  =  inportb  (BaseAddress  +  CHANNEL___SLCT)  ; 
B  =  B  &  240; 

B  =  B  I  (ChannelNumber  -  1) ; 

outportb ( BaseAddress  +  CHANNEL_SLCT ,  B) ; 

}  "  ’ 


/*  read  current  byte  */ 
/*  clear  BO  -  B3  */ 

/*  set  channel  bits  */ 
/*  write  new  byte  */ 


SetGain 
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The  SetGain  procedure  is  used  to  set  the  Gain  bits  on  the  DM5406. 

Note  "how  this  procedure  sets  only  the  channel  select  bits;  it  does  not 
change  the  other  bits  in  this  register.  This  is  important  because  if 
you  unintentionally  clear  the  other  bits  it  can  cause  unexpected 
behavior 
of  the  DM5406. 

****  +  **************  +  *****************  +  **************■*■****  +  ************/ 


void  atodClass :: SetGain (unsigned 


{ 

unsigned  char  Bi¬ 
switch  (Gain) 

{ 

case  1  :  Gain  =  0; 
case  2  :  Gain  =  1; 
case  4  :  Gain  -  2; 
case  8  :  Gain  =  3; 
default  :  Gain  =  0; 

} 


char  Gain) 


break; 

break; 

break; 

break; 

break; 


B  =  inportb (BaseAddress  +  GAIN_SLCT) ; 
B  =  B  &  207; 

B  =  B  |  (Gain  *  16) ; 

outportb (BaseAddress  +  GAIN_SLCT,  B)  ; 

} 

/************************************* 


/*  read  current  byte  */ 
/*  clear  B4,B5*/ 

/*  set  gain  bits  */ 

/*  write  new  byte  */ 


SetlRQSt-atus 

The  SetIRQStatus  procedure  is  used  to  set  or  clear  the  IRQ  Enable  bit 
on  the  DM5406.  A  value  of  1  passed  to  this  procedure  enables 
interrupts 

a  value  of  0  disables  interrupts. 


void  atodClass :: SetIRQStatus (char  IRQStatus) 

{ 

unsigned  char  B; 

B  =  inportb (BaseAddress  +  IRQ_ENABLE) ; 

B  =  B  &  127; 

B  =  B  I  IRQStatus  *  128; 
outportb (BaseAddress  +  IRQ_ENABLE,  B) ; 

} 

j  *  *  ***Jr**.  ★*★*********★**★**★*  +  +  *★**  +  *******★***★*******’*■■*’★**■*•*■**★★*■*■* 


/*  read  current  byte  */ 
/*  clear  B5  */ 

/*  set  IRQ  select  bit  */ 
/*  write  new  byte  */ 


SetExternalTrigger 

The  SetExternalTrigger  routine  is  used  to  set  the  external  trigger  bit 
on  the  DM5406.  A  value  of  1  passed  to  this  procedure  enables  the 
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external  trigger,  a  value  of  0  disables  the  external  trigger. 

IT*********************************************************************/ 

void  atodClass :: SetExternalTrigger (unsigned  char  TriggerStatus ) 

{ 

unsigned  char  B; 

B  =  inportb  (BaseAddress  +  EXT__TRIG_ENABLE)  ;  /*  read  current  byte  */ 

B  =  B  &  191;  /*  clear  B6  */ 

B  =  B  |  TriggerStatus  *  64;  /*  set  Trigger  bit  */ 

outportb (BaseAddress  +  EX  T  JT  R I  G_EN  ABLE,  B)  ;  /*  write  new  byte  */ 

} 

/********************************************************************** 
Set Interrupt Source 

The  SetlnterruptSource  procedure  determines  the  source  used  for 
generating 

interrupts  for  the  DM5406.  Passing  SOURCE_AD_START  selects  the  A/D 
start 

convert;  passing  SOURCE_DMA_DONE  selects  DMA  done;  passing 
SOURCE_TRIGGER 

selects  the  trigger;  finally,  passing  SOURCE_PACER_CLOCK 
to  this  procedure  selects  the  pacer  clock. 

**********************************************************************/ 

void  atodClass :: SetlnterruptSource (unsigned  char  Source) 

{ 

unsigned  char  B; 

B  =  inportb (BaseAddress  +  SCAN_BURST_SLCT) ;  /*read  current 

byte*/ 

B  =  B  &  207;  /*clear  bits  4  and 

5*/ 

B  =  B  |  Source  *  16;  /*set  new  bits*/ 

outportb (BaseAddress  +  SCAN_BURST_SLCT,  B) ;  /*write  new  byte*/ 

} 

/************************** ************** ****************************** 
ScanBurstEnable 

The  ScanBurstEnable  procedure  allows  the  user  to  enable  either  scan 
mode 

or  burst  mode.  A  value  of  0  passed  to  this  procedure  disables  both 
modes - 

a  value  of  1  enables  scan  mode;  a  value  of  2  enables  burst  mode. 

*  +  +  * ****************************************************************/ 
void  atodClass :: ScanBurstEnable (unsigned  char  Enable) 
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{ 

unsigned  char  B; 


switch  (Enable) 

{ 

case  DISABLE_BOTH:  Enable  =  0;  break; 
case  SCAN_ENABLE:  Enable  ==  2;  break; 
case  BURST_ENABLE :  Enable  =  3;  break; 
default:  Enable  =  0; 

} 

B  =  inportb  ( BaseAddress  +  SCAN_BURST__SLCT)  ; 
B  =  B  &  63; 

B  =  B  I  Enable  *  64; 

outportb  (BaseAddress  +  SCAN__BURST_SLCT,  B)  ; 


/*read  current  byte*/ 
/*clea£  bits  6  and  7*/ 
/*set  new  bits*/ 
/*write  new  byte*/ 


SetScanChannels 

The  SetScanChannels  procedure  determines  how  many  channels  are  scanned 
or 

bursted  while  in  the  appropriate  mode,  selected  using  ScanBurstEnable . 
The  parameter  passed  in  this  procedure  is  simply  the  number  of 
channels  the 
user  wishes  to  include. 

**********************************************************************/ 
void  atodClass :: SetScanChannels (int  NumChannels) 

{ 

unsigned  char  B; 

B  =  inportb (BaseAddress  +  SCAN_BURST_SLCT) ;  /*read  current 

byte*/ 

B  ~  B  &  240;  /*clear  bits  0 

through  3*/ 

B  =  B  |  (NumChannels  -  1);  /*set  new  bits*/ 

outportb (BaseAddress  +  SCAN_BURST_SLCT,  B) ;  /*write  new  byte*/ 

} 

/******************************************★*************************** 


Start Conversion 

The  StartConversion  procedure  is  used  to  start  conversions . 

**********  ******  ******************************************************/ 


void  atodClass: : StartConversion (void) 

{ 
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outportb (BaseAddress  +  START_CONVERSION,  0)  ; 

} 

/********************************************************************** 

ConversionDone 

The  ConversionDone  function  returns  TRUE  if  a  conversion  is  complete, 
FALSE 

if  a  conversion  is  in  progress. 

int  atodClass :: ConversionDone ( ) 

{ 

unsigned  char  Status; 

Status  =  inportb (BaseAddress  +  STATUS_BYTE) ;  /*  read  board  status 

*/ 

if  (  (Status  &  1)  ==  1) 

return  1;  /*  if  BO  is  set  return  TRUE  */ 

else 

return  0;  /*  if  BO  is  not  set  return  FALSE  */ 


ReadData 

The  ReadData  function  retrieves  two  bytes  from  the  converter  and 
combines  - 

them  into  an  integer  value. 


**********************************************************************/ 

int  atodClass : : ReadData (void) 

{ 

unsigned  char  MSB,  LSB; 
int  DigitalValue; 

MSB  =  inportb (BaseAddress  +  READ_JDATA)  ; 

LSB  -  inportb (BaseAddress  +  READ_DATA)  ; 

DigitalValue  =  (MSB  *  256  +  LSB)  ; 

return ( DigitalValue )  ; 

} 

/*****★**★******★*★★*★*****★********★★★★********★★****★******★★****■*'*■*'* 


ClockMode 

The  ClockMode  procedure  is  used  to  set  the  mode  of  a  designated 
counter 

on  the  8254  programmable  interval  timer  (PIT) . 
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void  atodClass :: ClockMode (unsigned  char  Clock,  unsigned  char  Mode) 

{ 

unsigned  char  StatusByte; 

StatusByte  =  (Clock  *  64)  +  (Mode  *  2)  +  48; 
outportb (BaseAddress  +  TIMER_CTRL,  StatusByte); 

} 


/********************************************************************** 

ClockDivisor  ~ 

The  ClockDivisor  procedure  is  used  to  set  the  divisor  of  a  designated 
counter  on  the  8254  programmable  interval  timer  (PIT) .  This  procedure 
assumes  that  the  counter  has  already  been  set  to  receive  the  least 
significant  byte  (LSB)  of  the  divisor  followed  by  the  most  significant 
byte  (MSB) . 


**********************************************************************/ 

void  atodClass :: ClockDivisor (unsigned  char  Clock,  unsigned  int  Divisor) 

{ 

unsigned  char  MSB,  LSB; 
unsigned  int  PortID; 

PortID  =  BaseAddress  +  TIMER_A  +  Clock; 

LSB  =  Divisor  %  256; 

MSB  =  Divisor  /  256; 
outportb (PortID,  LSB); 
outportb ( PortID,  MSB) ; 

} 

/**************************************  *********  *********************** 
SetUserClock 


The  SetUserClock  procedure  is  used  to  set  the  programmable  interval 
timer  (PIT)  on  the  DM5406  such  that  the  output  of  counter  2  goes  high 
at  the  specified  rate.  The  maximum  attainable  rate  this  procedure,  as 
written,  is  250,000  Hz  although  you  can  easily  change  this  by 
adjusting 

the  divisors  accordingly. 


void  atodClass :: SetUserClock (float  Rate) 
{  -  — 

ClockMode (0,  2); 

ClockDivisor ( 0 ,  8); 


ClockMode (1,  2); 

ClockDivisor (1,  (500000.0  /  Rate)); 


ClockMode (2,  2); 
ClockDivisor (2,  2)  ; 


SetPacerClock 

The  SetPacerClock  procedure  is  used  to  set  the  programmable  interval 
timer  (PIT)  on  the  DM5406  such  that  the  output  of  counter  1  goes  high 
at  the  specified  rate.  The  maximum  attainable  rate  this  procedure,  as 
written,  is  250,000  Hz  although  you  can  easily  change^ this  by 
adj  usting 

the  divisors  accordingly. 

Note  that  the  Pacer  and  User  clocks  are  really  the  same  device  and 
cannot  be  used  independently.' 

**********************************************************************/ 
void  atodClass :: SetPacerClock ( float  Rate) 


ClockMode (0,  2) ; 

ClockDivisor ( 0,  16)  ; 

ClockMode (1,  2); 

ClockDivisor (1,  (500000.0  /  Rate)  )  ; 


^********************************************************************** 
Read  DigitallO 

The  ReadDigitallO  function  returns  the  value  of  the  specified  digital 
input  port.  Each  digital  input  line  is  represented  by  a  bit  of  the 
return  value.  Digital  in  0  is  bit  0,  digital  in  1  is  bit  1,  and  so  on. 


unsigned  char  atodClass :: ReadDigitallO (unsigned  char  InputPort) 

{ 

return ( inportb (BaseAddress  +  PPI_A  +  InputPort)); 


WribeDigitallO 

The  Wr it eDigit al 10  function  sets  the  value  of  the  digital  output  port 
to 

equal  the  value  passed  as  parameter  v.  Each  digital  output  line  is 
represented  by  a  bit  of  v.  Digital  out  0  is  bit  0,  digital  but  1  is 
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bit  1 , 
and  so  on. 


void  atodClass : : WriteDigitallO (unsigned  char  OutputPort,  unsigned  char 
v) 

{ 

outportb (BaseAddress  +  PPI_A  +  OutputPort,  v) ; 

} 


Configure IOPorts 

The  Configure IOPorts  procedure  is  used  to  configure  the  ports  A  and  C 
on 

the  8255  PPI  for  either  input  or  output.  A  value  of  1  means  input,  a 
value 

of  0  is  for  output.  It  is  advisable  to  use  the  INPUT  and  OUTPUT 
constants 

defined  in  this  file. 

Port  B  remains  set  for  output. 


************* *********************************************************^ 

void  atodClass :: Conf igurelOPort s  (unsigned  char  PortA,  unsigned  char 
PortC) 

{ 

unsigned  char  ControlByte; 

ControlByte  =  128  +  (PortA  *  16)  +  (PortC  *  9) ; 
outportb (BaseAddress+PPI_CTRL,  ControlByte) ; 

} 

/****************************  ****************************************** 
UpdateDAC 

The  UpdateDAC  procedure  outputs  the  specified  voltage  to  the  specifed 
DAC.  The  DACSlope  and  DACOffset  variables  must  be  set  to  the  values 
required  for  the  output  range  of  the  DACs. 

***********************************************  ***********************/ 


void  atodClass :: UpdateDAC (unsigned  char  DAC,  float  Volts) 

{ 

int  Value; 

Value  =  (int)  (Volts  *  DACSlope)  +  DACOffset; 

outportb (BaseAddress  +  DAC1_LSB  +  (DAC  -  1)  *  2,  Value -%  256); 
outportb  (BaseAddress  +  DAC1__MSB  +  (DAC  -  1)  *  2,  Value  /  256); 
outportb (BaseAddress  +  DAC_UPDATE,  0); 

} 

//end  atod.cpp 
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APPENDIX  B.  MATLAB  CODE  FOR  SANS  KALMAN  FILTER 

clear  all 
format  long 

gp  s_f 1 a g_t ime= 1 ; 
cum_time=0 ; 
x_hat=zeros (8,1); 

simulation_time=5 . 0;  %  in  minutes 

samples= (simulation_time  *  60)  /  0.025;  %  0.025  is  the  average  delta_t 

heading=6. 2832;  %  for  north 

speed=10; 

x_hat_plot=zeros  (samples,  9)  ;  ' 

temp=zeros (1,9); 

%  Time  Constants 

tau_l  =  10;  %  seconds  for  velocity 

tau_2  -  3600;  %  seconds  for  current 

tau_3  =  60;  %  seconds  for  GPS 

%R1  matrix 

rl= [ . 01  0  0  0  ;  0  .01  0  0 
0000;  0000]; 

%R2  matrix 

r2= [ . 01  0  ;  0  .01] ; 

%  P_minus  matrix 

p_minus  =[.l  0  0  0  0  0  0  0; 

0.1000000  ; 

00. 100000  ; 

000. 10000  ; 

0000. 1000  ; 

00000. 100  ; 

000000. 10  ; 

0  0  0  0  0  0  0.1]; 

%  P  matrix 

p  =  [ .  5  0  0  0  0  0  0  0  ; 

0. 5000000  ; 

0  01. 0  0  0  0  0  0  ; 

0  0  01. 0  0  0  0  0  ; 

0  0  0  010.0  0  0  0; 

0  0  0  0  010.0  0  0; 

0  0  0  0  0  015.0  0; 

0  0  0  0  0  0  0  15.0] ; 


%H1  matrix 

hl=.[  1  JD  0  0  0  0  0  0  ; 
01000000  ; 
0D001010; 
Ch  0  000  101]; 
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%H2  matrix 

h2=[l  0000000; 

0  1  O'  0  0  0  0  0]  ; 

D1=0. 01; 

D2=0 . 01 ; 

D3=0 . 5 ; 

Dl_p  =  0.01; 

D2_p  =  0.01; 

D3_p  =  0.5; 

%  initial  process  state 

x_previous  =  [0;  0;  0;  0;  0;  0;  0;  0]  ; 

%for  plotting  only. 
gps_plot  =  [  0;  0]; 

beginning  kalman  filter  loops* 

for  i=l: samples 

delta_t=0 . 02;  %  0.01*rand 

gps__f  lag_time=gps_flag__time+delta_t  ; 

cum_t  ime=cum__time+delta__t  ; 

%  construct  Phi  matrix 
uu=exp (-delta_t/tau_l) ; 
vv=exp (-delta_t /tau_2 ) ; 
ww=exp  (-delta_t/tau__3)  ; 

phi  =[uu0000000; 

0  uu  0  0  0  0  0  0  ; 

00  vv  00000; 

0  0  0  vv  0  0  0  0  ; 

0000  ww  000; 

00000  ww  00; 

tau_l*(l-uu)  0  tau_2*{l-vv)  00010; 

0  tau_l* (1-uu)  0  tau_2* (1-vv)  0001]; 

%construct  Q  matrix  for  Kalman  filter 
xx=exp ( (-2*delta_t) /tau_l) ; 
yy=exp ( (-2*delta_t) /tau_2) ; 
zz=exp ( (-2*delta_t ) /tau_3) ; 

Q00= ( (Dl/ (2*tau_l) ) * (xx) ) ; 

Qll=( (Dl/ (2*tau_l) ) * (xx) ) ; 

Q22= ( (D2/ (2*tau_l) )*(yy) ) ; 

Q33= ( (D2/(2*tau_l) ) * (yy) ) ; 

Q4  4= ( ( D3/ (2*tauJL) ) * (zz) ) ; 

Q55— (-(D3/  (2*tau_l)  )  *  (zz)  )  ; 

Q661=D1* (delta__t-2*tau_l* (1-uu) + (tau_l/2) * (1-xx) ) ; 
Q662=D2* (delta_t-2*tau_2* ( 1-vv) + ( tau_2/2 ) * (1-yy) ) ; 
Q66=Q661+Q662 ; 

Q77=D1* (delta_t-2*tau_l* (1-uu) + (tau_l/2) * (1-xx) ) +D2 
* (delta_t-2*tau_2* (1-vv) + (tau_2/2) * (1-yy) ) ; 
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Q06=D1* ( (1/2) - (uu)+ (1/2) *xx) ; 
Q60=Q06; 


Q2  6=D2* ( (l/2)-(vv)  +  (l/2)*yy) ; 
Q62=Q26; 

Q17=Q06; 

Q71=Q17 ; 

Q37=Q62 ; 

Q73=Q37; 


%Q  matrix 

q  -  [QOO  00000 
0  Qll  0  0  0  0 
0  0  Q22  0  0  0 
0  0  0  Q33  0  0 
0  0  0  0  Q44  0 
0  0  0  0  0  Q55 
Q60  0  Q62  0  0 
0  Q71  0  Q73  0 


Q06  0  ; 

0  Q17  ; 
Q26  0  ; 

0  Q37  ; 

0  0  ; 

0  0  ; 

0  Q66  0; 

0  0  Q77]  ; 


%construct  Q  matrix  for  process  model 
xx=exp ( (-2*delta_t ) /tau__l) ; 
yy=exp ( (-2*delta_t ) /tau_2 ) ; 
zz=exp ( (-2*delta_t ) /tau_3) ; 

Q00= ( (Dl_p/(2*tau_l) )* (xx) ) ; 

Qll= ( (Dljp/ (2*tau_l) ) * (xx) ) ; 

Q22= ( (D2_p/ (2*tau_l) ) * (yy) ) ; 

Q33= ( (D2_p/ (2*tau_l) ) * (yy) ) ; 

Q4  4= ( ( D3_p/ (2*tau_l ) ) * (zz) ) ; 

Q55- ( (D3_p/ (2*tau_l) ) * (zz) ) ; 

Q6 6 l=Dl_p* (delta_t-2*tau_l* ( 1-uu) + (tau_l/2 ) * ( 1-xx) ) ; 
Q662=D2_p* (delta_t-2*tau_2* (1-vv) + (tau_2/2) * (1-yy) ) ; 
Q66=Q661+Q6  62 ; 

Q77-Dl_p* (delta_t-2*tau_l* ( 1-uu) + ( tau_l/2 ) * ( 1-xx) ) +D2_p 
* (delta_t-2*  tau_2* (1-vv) + (tau_2/2) * (1-yy) ) ; 
Q06=Dl_p*  (  (1/2)  -  (uu)  +  ( 1/2 )  *xx)  ; 

Q60-Q06; 

Q26=D2_p*  (  (l/2)-(w)  +  (l/2)*yy)  ; 

Q62=Q26 ; 

Q17=Q06; 

Q71=Q17 ; 

Q37=Q62 ; 

Q7  3=Q37 ; 


q_process 


[QOO  00000 
0  Qll  0  0  0  0 
0  0  Q22  0  0  0 
0  0  0  Q33  0  0 
0  0  0  0  Q44  0 
0  0  0  0  0  Q55 
Q60  0  Q62  0  0 
0  Q71  0  Q73  0 


Q06  0  ; 

0  Q17  ; 

Q2  6  0  ; 

0  Q37  ; 

0  0  ; 

0  0  ; 

0  Q66  0; 

0  0  Q77 ] ; 


%  process  model  noise 
white_noise  =  randn(8,l); 
w  -  sqrtm  (q_process )  *  white__noise; 

w(l)=w(l)+0.02;  %  input  to  generate  some  speed 
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x  ='  phi  *  x_previous  +  w; 
x_previous  =  x; 

%  measurement  noise  from  water  speed  sensor 
v  -  0.2*randn(2, 1)  ; 

%  those  two  equations  are  here  for  smoothing  rather  than  prediction. 
x_hat_minus=phi*x_hat ; 
p_minus=  (phi*p*phi 1 ) +q; 

tmp  ( 1 )  =  cum_t  ime  ; 

if  (gps_f lag_time>=l) 

z  withjgps  =  hi  *  x;  %measurement  with  GPS 
,z_with_gps  (1)  =  z_with_gps  (1)  +  v(l); 
z_with_gps (2)  =  z_with_gps  (2 )  +  v(2); 

z_with_gps (3)  =  0.0  +  x(5); 
z_with_gps  (4)  =  0.0  +  x(6); 

%  k  1  =p__m i nu s * h  1 1  *inv  {  (hl*p_minus)  *hl 1  +rl)  ; 
kinv=inv ( (hl*p_minus) *hl T+rl) ; 
kl=p_minus*hl 1 *kinv; 

ii=ii+4 ; 

x_hat=x_hat__minus+kl* (z_with_gps- (hl*x_hat_minus ) ) ; 
p= (eye ( 8 ) -kl*hl ) *p_minus; 

gps_plot ( 1 )  =  z_jwith_gps (3) ; 
gpsjplot(2)  =  z_with_gps (4 ) ; 

%  for  printing  purpose 
for  j  — 1 : 4 

tmp  ( j  + 1 )  =  z__with_gps  ( j  )  ; 

end 

gps_f lag_time=0 ; 
else 

z  no_gps  =  h2  *  x;  %  measurement  without  GPS 

z__no_gps  =  z_no__gps  +  v; 

%  k2=p_minus*h2 ' *inv( (h2*p_minus) *h2'+r2) ; 
k2inv=inv ( (h2*p_jninus ) *h2 1 +r2) ; 
k2=p_minus*h2 '  *  k2inv; 
k2invtmp= [ k2inv (1,1)  k2inv(l,2)  0  0; 

k2inv(2,l)  k2inv(2,2)  0  0]; 

x_hat=x_hat_minus+k2* ( z_no_gps- (h2*x_hat_minus ) ) ; 

_p=(eye (8)-k2*h2) *p_minus; 
for  j  =4 : 5 

tmp(j)  =  gps_plot ( j-3) ; 

end 
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for  j~2:3 

tmp(j)  =  z_no_gps  ( j-1) ; 

end 

end  %end  if  statement 

%  collecting  measurement  for  plotting  purpose 
zjolot (i,  : )  =  tmp; 

%  collecting  x_hat  for  plotting  purpose 
temp 2 (1) =cum_time; 
temp 3 (1) =cum_time; 
for  j  =2 : 9 

temp2  ( j  )  =x__hat  ( j-1 )  ; 
temp3 ( j ) =x ( j-1 ) ; 

end 

x_hat_plot  ( i ,  :  )  =temp2; 
x_plot(i,:)  =  temp3; 

end  %end  of  Kalman  Filter  loop 
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